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FOREWORD 


This  report  presents  an  investigation  of  two  topics  related  to  mitigating  the  effects  of  radar 
bias  in  tracking  applications  including  hallistic  tracking.  We  determine  the  absolute  bias  between 
two  radars  in  polar  coordinates  when  their  relative  bias  is  given  in  rectangular  coordinates.  Using 
this  result,  the  optimized  steady-state  filter  to  handle  the  unknown  deterministic  bias  is  then 
obtained. 

An  earlier  version  of  this  report  was  published  at  the  2008  National  Fire  Control  Symposium, 
Lexington,  Massachusetts. 

This  report  has  been  reviewed  by  Paul  H.  Wingeart,  Head,  Ballistic  Missile  Defense  Systems 
Engineering  Branch,  and  Gilbert  W.  Goddin,  Head,  Warfare  Systems  Engineering  and  Analysis 
Division. 
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NOMENCLATURE 
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x{k  +  l\k) 

X  {k\k) 
z{k) 
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{k\k) 
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K{k) 


a,  (3 
P 


State,  n-dimensional,  (3-1)^ 
Time  updated  state  at  k,  (3-3) 
Measurement  updated  state  at  k,  (3-4) 
Measurement  at  k,  g-dimensional,  (3-2) 
Process  noise  at  k,  n-dimensional,  (3-1) 
Measurement  noise  at  k,  g-dimensional,  (3-2) 
State  transition  matrix  from  k  to  I,  n  by  n-dimensional,  3-1^ 
Noise  input  matrix  at  k,  n  by  6-dimensional,  3-1 
Output  matrix  at  k,  q  by  n-dimensional,  3-1 
Process  noise  intensity  at  k,  n  by  n-dimensional,  3-1 
Measurement  noise  intensity  at  k,  q  by  q-dimensional,  3-1 

Bias,  p-dimensional,  (3-2) 
Mean  and  covariance  of  A,  3-1 
Bias  function,  K""  x  to  K*',  (3-2) 
Bias  matrix,  q,  r-dimensional,  (3-2) 
Total  error,  (3-5) 
Error  due  to  noise,  (3-18) 
Error  due  to  bias,  (3-19) 
Total  error  covariances,  3-5 
covariances,  (2-20)  and  (3-21) 
Kalman  filter  gain  matrix,  n  by  (7-dimensional, 

(3-4)  and  (3-34) 

Steady  state  position  and  velocity  Kalman  filter  gains,  (3-49) 

Target  maneuver  index,  3-22 


indicates  equation  number. 
^Indicates  page  number. 
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1  INTRODUCTION 


There  are  several  facets  to  the  problem  of  tracking  missiles  that  engage  in  deterministic 
maneuvers  with  radar.  Deterministic  maneuvers  can  be  accounted  for  with  bias  estimation 
techniques  that  enable  enhanced  error  correction  to  be  applied  to  tracking  algorithms,  which 
leads  to  more  effectively  tracked  threats.  In  this  report,  we  obtain  the  exact  form  of  the  bias 
error  for  the  coordinate  transformation  problem.  This  result  is  useful  in  Ballistic  Missile 
Defense  bistatic  applications  where  one  sensor  is  used  for  launching  an  interceptor,  while 
another  is  used  to  track  the  threat.  Thus,  it  is  important  to  address  the  problem  of 
translation  between  internal  sensor  coordinate  frames  to  a  common  frame  that  is  used  by  all 
sensors.  The  coordinate  transformation  problem  from  Cartesian  to  spherical  coordinates 
introduces  a  bias  that,  if  accounted  for,  can  be  corrected  in  the  filter  design.  This 
transformation  problem  occurs  when  there  are  multiple  launch  platforms,  as  each  local  track 
must  be  formatted  for  a  common  reference  frame.  When  bias  correction  is  accomplished 
correctly,  one  can  improve  the  tracking  performance  of  the  filter  and  increase  the  likelihood 
that  an  interceptor  can  successfully  engage  a  threat. 

The  results  presented  in  this  report  are  original  and  are  due  to  the  authors.  An  earlier 
version  of  this  report  appeared  in  [10],  which  is  also  archived  in  www.arXiv.org. 
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2  AN  OPTIMIZED  METHOD  OF  OBTAINING  ABSOLUTE  BIAS 


Although  a  relative  bias  calculation  can  be  used  to  provide  the  correct  association  of 
tracks  from  two  sensors,  calculation  of  the  absolute  bias  is  required  to  correct  the  track  state 
and  is  needed  for  both  track  fusion  and  producing  a  Single  Integrated  Air  Picture.  Levedahl 
[4]  and  Brown,  Weisman,  and  Brock  [5]  present  methods  for  obtaining  the  relative  bias 
between  two  radars  tracking  the  same  ballistic  missile,  which  include  maximizing  a  likelihood 
function.  The  relative  biases  obtained  in  these  papers  are  determined  in  rectangular 
coordinates.  In  this  report,  the  absolute  bias  for  the  two  sensors  is  calculated  from  the 
relative  bias  by  solving  a  minimization  problem.  The  absolute  biases  of  the  two  sensors  are 
given  in  spherical  coordinates.  The  problem  is  set  up  to  minimize  the  weighted  sum  of  the  two 
absolute  biases  while  viewing  the  given  relative  bias  as  a  constraint. 

A  point  in  three-dimensional  space  in  both  rectangular  and  spherical  coordinates^  is 
denoted  by 


'p  = 

X 

y 

and  ^  = 

r 

,  respectively. 

(2-1) 

z 

e 

The  transformations  between  the  coordinates  are  ~p  =  /(^)  and  ^  =  /  ^(^),  which  are 
given  by 


r  cos  9  cos  V’ 

+  1/“^  +  z'^ 

/(^)  = 

r  cos  6  sin  t/i 

and  /  H'P  )  = 

arctan  (y/x) 

r  sin0 

arctan  [zj  ■sjx’^  -|-  j 

We  need  the  following  definitions: 


Pi 

Target  position  as  seen  by  sensor  1 

P2 

Target  position  as  seen  by  sensor  2 

Pt 

True  target  position  (unknown) 

Pi 

Sensor  1  bias 

P2 

Sensor  2  bias 

Br 

Relative  bias 

PlT02 

Sensor  2  position  from  sensor  1 

Let  ENU  denote  the  East  North  Up  coordinate  system.  We  have 
Pl,ENU{l)  =  P2,ENU{2)  =  {X2,y2,  Z2yENU{2)^ 

Bi,ENUii)  =  (A®!,  Ayi,  and  B2^enu(2)  =  (Ax2,  Ay2,  A2;2)^^,y(2)-  ^hus,  in  the 

sensor  coordinates 

Pt,ENU(1)  =  Pl,ENU{l)  +  Bi^eNU{1)  (2-3) 

Pt,ENU{2)  =  P2,ENU{2)  +  B2^ENU{2)  ■  (2-4) 


^Denote  yaw  (azimuth)  by  ip,  pitch  (elevation)  by  d.  (p  is  normally  reserved  for  roll;  however,  roll  is  not  used 
here. 
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If  we  use  an  ENU  coordinate  system  located  at  sensor  1,  (2-4)  becomes^ 

Pt,ENU{1)  =  PlT02,ENU{l)  +  P2,ENU{1)  +  B2,ENU{1)  (2-5) 

where  Pito2,enu{i)  is  the  position  vector  from  the  first  sensor  to  the  second  sensor  in 
ENU(l).  The  relative  bias  in  ENU(l)  is 


Br,enu{i)  —  B2,enu{i)  —  Bi^enu{i) 

=  {Pt,ENU{1)  -  PlT02,ENU{l)  “  P2,ENU{1))  “  {Pt,ENU(1)  “  Pl,ENU(l)) 
=  Pl,ENU{l)  —  PlT02,ENU{l)  “  P2,ENU{1)  ■ 


(2-6) 


We  consider  the  coordinate  transformations  to  allow  us  to  go  from  ENU  to  radar-face 
coordinates  for  a  particular  sensor.  Each  sensor  has  its  own  face  and  ENU  coordinate 
systems.  A  sensor’s  face  coordinate  system  (EACE)  is  related  to  the  ENU  coordinate  system 
of  a  sensor  by  the  following  transformation: 


^ENU{i)2FACE{i) 


cos  9i 

0 

sin  9i 

cos  t/’j 

sin 

0  ■ 

0 

1 

0 

—  sint/^j 

cos 

0 

—  sin  9i 

0 

cos  9i 

0 

0 

1 

9i  cos  t/jj 

cos  9i  sin 

sin  9i 

-  sin  t/ij 

cos 

0 

—  sin  9i  cos  '0  j  —  sin  9i  sin  cos  9i 
where  i  =  1,2.  We  also  have  that 


(2-7) 


FACE{i)2ENU(i) 


COS  9i  COS  'i/’j  —  sin  -0^  —  sin  9i  cos 

cos  9i  sin  j  cos  ^|J^  —  sin  9i  sin  V'j 

sin  9i  0  cos  9i 


(2-8) 


which  is  the  transpose  of  (2-7).  We  can  also  have  the  matrix  TENU(i)2FACE{j)i  which  is 


TENU{i)2FACE{j) 


COS  9ij  cos  V’i  j 
-sin'i/’ij 
—  sin  9ij  cos 


cos  9ij  sin  j  sin  9ij 
cos  tpij  0 

—  sin  9ij  sin  V'*  j  cos  9ij 


(2-9) 


The  absolute,  as  opposed  to  relative,  bias  can  be  expressed  in  the  EACE  coordinates: 


Bi,FACE{i)  =  Ar  •  -h  Acyi  •  +  Acb  ■  (2-10) 

where  iC  is  the  unit  vector  in  the  range  coordinate  and  ud,  UcB  are  the  two  cross  range 
coordinate  unit  vectors.  Substituting  ptA'i/j  =  Aca  and  ptA9  =  Acb  where  pxi  =  ||Et(*)|| 

(see  note^),  the  distance  from  the  sensor  to  the  target,  we  get 

Bi,FACE{i)  =  Ari-^r+  PTiAlpi  ■  +  PTiA9i  •  (2-11) 


^Appendix  A  gives  the  transformation  from  ENU(l)  to  ENU(2). 

^True  position  is  not  available;  therefore,  measured  position  is  used  when  applying  this  method  to  this 
calculation. 
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Bi,ENU{i) 


cos  9i  cos 

—  sin  V’j 

—  sin  9i  cos  V’i 

Ari 

cos  9i  sin  'i/ij 

cos  ^|J^ 

—  sin  9i  sin  '0^ 

PTiA-lpi 

sin  9i 

0 

cos  9i 

_  PTiA9i 

Avi  cos  9i  cos  V'i  —  Aip^  (sin  ip^)  ■  pxi  —  AOi  (sin  9i  cos  'ijjj)  ■  pxi 
Ari  cos  9i  sin  +  A'0j  (cos  'ipP)  ■  pxi  —  A9i  (sin  9i  sin  V’*)  •  Pn 
Avi  sin  9i  +  A9i  (cos  9i)  ■  pn 


(2-12) 


We  can  obviously  obtain  Bi  ENU{j)  (for  *  riot  necessarily  equal  to  j)  if  needed.  The  quantities 
Ari,  A^’i,  A01,  Ar2,  A^2)  ^^2  are  minimized.  When  referring  to  these  terms  as  a  group  we 
write  e  =  (Ari,  AV’i,  A0i,  Ar2,  Aip2^  A92)- 

We  need  tolerances  or  costs  for  the  sensor  biases.  These  costs  are  expressed  in  spherical 
coordinates. 


krl 

Sensor  1  range  bias  cost,  unitless 

kr2 

Sensor  2  range  bias  cost,  unitless 

Sensor  1  azimuth  bias  cost,  meters 

Sensor  2  azimuth  bias  cost,  meters 

kei 

Sensor  1  elevation  bias  cost,  meters 

ke2 

Sensor  2  elevation  bias  cost,  meters 

Problem  Statement 


We  want  to  compute  the  minimum  (absolute)  bias  cost  for  the  two  sensors  when  there  are 
known  (computed)  expressions  for  the  relative  bias.  The  given  relative  bias  is  expressed  in 
ENU  rectangular  coordinates.  We  compute  the  minimum  absolute  bias  in  spherical 
coordinates.  The  relative  bias  in  rectangular  coordinates  contrasted  with  the  absolute  bias  in 
spherical  coordinates  allows  us  to  formulate  this  as  a  minimization  problem.  We  view  the 
relative  bias  as  a  constraint.  We  use  a  quadratic  cost: 


(Ari)2  + 


{Acp.f  +  ^  •  {A9,f  +  ^  •  {Ar^f  +  ^ 


{AcP2?  +  ^-^-{A92f  .  (2-13) 


So  that  the  addition  in  (2-13)  is  permissible,  we  have  that  and  are  unitless  and  k^^, 
ken  k^^,  and  ke2  are  in  meters.  We  note  F  may  be  rewritten  in  the  form 


■  2/fc2^  0  0  ■ 

-1 

Ari 

os 

<1 

-A 

<1 

s- 

<1 

II 

0  2/4  0 

A'l/’i 

1 

0 

0 

to 

1 _ 

.  . 

■  2/kl^  0  0  ■ 

-1 

Ar2 

-1-  [  Ar2  A'02  A02  ] 

0  2/A:2^  0 

A'02 

_  0  0  2/fc2^  _ 

_  A02  . 

which  we  recognize  as  being  in  the  form  of  a  Mahalanobis  distance.  The  works  by  Levedahl  [4] 
and  the  Lincoln  Laboratory  [5]  include  the  Mahalanobis  distance  when  the  log  is  taken  of  the 
Gaussian  distribution.  The  cost  F  is  minimized  subject  to  the  equality  constraint 


GiB)  —  {B2^enu(i)  —  Bi^ENUii))  —  Br^enu{i)  —  0  • 


(2-15) 
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Thus, 


GiB) 


Ar2  cos  02  cos  -02  “  ^'4’2  (sill  "02 )  ■  PT2  —  A9 2  (sin  62  cos  ^^2 )  ‘  PT2 
Ar2  cos  02  sin  1^2  +  A'02  (cos  ^p2)  •  PT2  —  A02  (sin  02  sin  '^2)  '  PT2 
Ar2  sin  02  +  A02  (cos  02)  ■  Pt2 


Ari  cos  01  cosV’i  —  Aipi  (sinV’i)  •  Pti  —  A0i  (sin0i  cos'i/'i)  •  Pti 
Ari  cos  01  sin  'ipi  +  A'i/’i  (cos  'ipi)  ■  pxi  —  A0i  (sin  0i  sin  ?/>i)  •  pxi 
Ari  sin  0i  +  A0i  (cos  0i)  •  pTi 


Br  =  0  (2-16) 


where  all  the  terms  in  (2-16)  reside  entirely  in  one  of  the  two  ENU  coordinate  systems.  We 
see  that  G{B)  gives  that  the  difference  between  the  two  absolute  biases,  whatever  they  may 
be,  is  equal  to  the  relative  bias.  Also,  note  that  (2-16)  is  affine.  Another  equivalent 
representation  for  G{B)  is 


where 


and 


Ar2 

Ari 

G{B)  =  A{pT2,'lp2^02) 

A02 

-  A(pti,0i,0i) 

A01 

_  A02  _ 

.  . 

A{PTl,'4^l,0l) 


COS  01  COSIpi 
cos  01  sin?/>i 
sin  01 


—  sin  'ijji  ■  Pti 
cos  V'l  •  Pti 


0 


—  sin  01  cos'f/'i  •  Pti 

—  sin  01  sini/^i  •  pTi 

cos  01  •  Pti 


A  {pt2,  ^/’2)  ^2) 


cos  02  cos  '02 
cos  02  sin  0)2 
sin  02 


-  sin  0)2  •  PT2 
cos  0)2  •  PT2 


0 


—  sin  02  cos  0)2  •  Pt2 

—  sin  02  sin  ■02  •  pt2 

cos  02  ■  PT2 


Setting  G{B)  =  0,  we  solve  for  Ar2,  A0)2,  A02  using 


Ar2 

A0)2 

A02 


=  A  ^  (pr2,0'2)^2) 


( 

Ari 

A(pri,0i,0i) 

A01 

V 

_  A01  _ 

(2-17) 


(2-18) 


(2-19) 


(2-20) 


(provided  pT2  7^  0).  The  vector  equality  constraint  (2-16)  can  be  written  in  the  form  of  three 
scalar  equality  constraints. 


Ge{B)  =  Ar2  cos  02  cos  0)2  —  A02  sin  02  ‘  PT2  —  A02  sin  02  cos  02  '  PT2 

—  Ari  cos  01  cos  01  -|-  A0;^  sin0;^  •  pTi  +  A0i  sin0i  cos0i  •  pTi  —  Bre  j  (2-21) 

Gn{B)  =  Ar2  cos  02  sin  02  -|-  A02  cos  02  •  Pt2  —  AO2  sin  02  sin  02  •  pt2 
—Ari  cos  01  sin  0;^  —  A0;^  cos  0i  •  pTi  +  A0i  sin  0i  sin  ■  pTi  —  Brn  ,  and  (2-22) 

Gjj{B)  =  Ar2  sin  02  -|-  A02  cos  02  •  Pt2  —  Ari  sin  0i  —  A0i  cos  0i  •  pTi  —  Brr  .  (2-23) 
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Solving  the  Minimization  Problem 


To  solve  this  minimization  problem,  we  need  to  take  a  few  derivatives.  We  need  the 
gradient  of  the  function  that  is  minimized.  We  also  need  the  gradient  of  the  constraint,  which 
is  an  equality  constraint  in  this  case. 


dF/dAri 

dF/dAi/ji 

dF/dA9i 

kl  •  A01 

dF/dAr2 

kr^  •  Ar2 

(9F/0A02 

kl^  •  A02 

_  dF/dA92  _ 

_  kl  •  A02  _ 

'^Ge  = 


VGn  = 


VGu 


dGE/dAri 

dGE/dA-ipi 

dGE/dAOi 

dGE/dAr2 

dGEldA'ijj^ 

_  BGe /dAe2  _ 

BGn  /  dAri 
OGn /dAi/^i 
dGN/dA9i 
BGn /dAr2 
dGN/dA'tJj2 
_  BGn /dAe2  _ 

dGu/dAri 
dGu/dAi;^ 
dGu/dAOi 
dGu/dAr2 
dGEldA'^2 
_  dGuldAQ2  _ 


—  cos  9\  cos 
sin  •  pTi 

sin  9i  cos  -01  •  Pti 
cos  02  cos  V’2 
-sin '02  ■PT2 

—  sin  02  cos  'i/^2  ■  PT2 

—  cos  01  sin0)]^ 

—  cos  01  •  Pti 
sin  01  sin0]^  •  pTi 

cos  02  sin  02 
cos  02  •  PT2 

—  sin  02  sin  02  •  pt2 

—  sin  01 
0 

—pTl  ■  COS  01 
sin  02 
0 

PT2  ■  COS  02 


(2-24) 


(2-25) 


(2-26) 


(2-27) 


We  are  looking  for  an  optimal  solution  located  at  the  point 

e*  =  (Ar^,  A0^,  A9\,  Ar^,  A02,  A02).  We  employ  the  Kuhn- Tucker  conditions  that  stipulate 
the  optimal  solution  e*  should  satisfy  these  equality  constraints  for  e,  and  there  exist  numbers 
a\,a*2,  Cg  such  that 


VF  (e*)  =  0*1  •  VGe  (e*)  +  ■  VGn  (e*)  +  ■  VGu  (e*)  .  (2-28) 


The  gradients  VGg  (e*) ,  VGat  (e*) ,  VGjj  (e*)  are  linearly  independent.  Taking  an  inventory 
of  the  equations  and  unknowns,  we  see  that  there  are  nine  unknowns  (6,01,02,03)  and  nine 
equations  [three  from  the  equality  constraint  and  six  from  (2-28)] .  We  may  be  able  to  find  the 
solution.  Since  the  cost  F  is  quadratic  and  the  constraint  G  is  affine,  the  necessary  conditions 
we  give  for  optimality  are  also  sufficient  conditions,  and  an  optimal  solution  e*  is  a  global 
optimal  solution. 
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Equation  (2-28)  in  longhand  is: 


Ari 

—  cos  0  cos  01 

—  cos  01  sin  01 

A01 

sin  01  •  Pti 

—  cos  01  •  Pti 

A01 

sin  01  cos  01  •  Pti 

+  02 

sin  01  sin  01  ■  pti 

Ar2 

=  Ol 

cos  02  cos  02 

cos  02  sin  02 

A02 

-  sin  02  •  PT2 

cos  02  •  PT2 

.  ^^2 

A02  _ 

—  sin  02  cos  02  •  Pt2 

—  sin  02  sin  02  •  Pti 

+03 


(2-29) 


—  sin  01 
0 

—PTI  ■  COS  01 
sin  02 
0 

PT2  ■ COS  02 

The  right  hand  side  of  (2-29)  may  be  written  in  the  form  of  the  product  of  two  matrices,  Mi 
and  M2. 


—  cos  0  cos  01 

—  cos  01  sin  01 

—  sin  01 

sin  01  •  PTl 

—  cos  01  •  Pti 

0 

sin  01  cos  01  ■  Pti 

sin  01  sin  0i  •  pTi 

—pTl  ■  COS  0 

cos  02  cos  02 

cos  02  sin  02 

sin  02 

-sin 02  ■PT2 

cos  02  •  PT2 

0 

—  sin  02  cos  02  •  Pt2 

—  sin  02  sin  02  •  Pt2 

PT2  ■  COS  02 

Ol 

'Ml' 

01 

02 

— 

M2 

02 

.  . 

.  . 

where 


Ml 


—  cos  0  cos  'ipi  —  cos  01  sin  V’l 
sim/ji  ■  pti  -costpi-pT 

sin  01  cos  'i/'i  •  Pti  sin  0i  sin  -i/^i  •  pT 


—  sin  01 
0 

—pTl  •  COS  01 


and 


M2 


cos  02  COS  'lp2 
-PTl 

—  sin  02  cos  '02  •  Pti 


cos  02  sin  '02 
cos  02  •  P2r 
—  sin  02  sin  02  •  P2T 


sin  02 
0 

PT2  •  cos  02 


Note  that  oi,  02,  and  03  have  the  units  of  meters.  Let 


(2-30) 


(2-31) 


(2-32) 


Di 


■  0  O' 

0  0 

0  0 


D2 


'  0  O' 

0  kl^  0 

L  0  0  kl  \ 


(2-33) 


(2-34) 


2-6 


NSWCDD/TR-12/555 


Rewriting  the  left  hand  side  of  (2-29)  as 
0 


we  have 


or, 


kri 

0 


0 

0 


0 

0 

0 

0 


0 

0 

0 

0 


k 


2 
ei 

0  kl 


0 

0 


r2 

0 


0 

0 

0 

0 


0  0 


0 

0 

0 

0 

0 


Ari 

A'01 

A01 

Ar2 

A'tp2 

_  A02  . 

Dl  03,3 
03,3  D2 


'Ml 

ai 

Ah 

02 

.  “3  . 

Ari 

Oi 

Dl 

AV’i 

=  Ml 

02 

.  A01  _ 

.  «3  . 

Ar2 

01 

D2 

A'02 

=  M2 

02 

_  A02  . 

.  ®3  . 

Ar2 

Ari 

A'i/'2 

=  D2^M2M{^Di 

Aipi 

.  ^^2  . 

AOi 

Substituting  (2-38)  into  (2-20)  yields 


D2^M2M{^Di 


Ari 

AV^i 

A01 


=  A  (pt2,'i/^2,^2)  ^A(pTl,'l/^i,0l) 


Ari 

AV>i 

A01 


so  we  get 

(L»2-^M2Mf  (PT2,  ^2,  ^2)  A  (PT1,^1,  0l)) 


Ari 

AV^i 

A01 


Ari 

A'01 

A01 

Ar2 

AV'2 

A02 


+  -B_r 


(2-35) 


(2-36) 


(2-37) 


(2-38) 


(2-39) 


=  A  {pt2,'4’2^^2)  Br  (2-40) 


Ari 

AV^i 

A01 


[D2^M2M^^Di  —  A  ^  (pt2,  ^/’2)  ^2)  ^  (PTl,  V'l)  ^1))  ^  ^  {PT2-i'4^2-:^2)  Br  , 


(2-41) 

which  allows  us  to  obtain  (Ari,  A'i/^i,  A0i).  Finally,  substituting  (2-41)  into  (2-38)  we  get 

(Ar2,  AV’2,  A6'2). 
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Numerical  Examples 

The  examples  below  illustrate  this  idea. 


Example  A 


The  input  for  this  case  is  a  variation  of  the  input  in  Example  A.  Note  that  the  output  for 
this  case  is  the  same  as  Example  A,  with  the  exception  of  a  sign  swap  between  A'ijj2  and  A02 
to  account  for  the  orientation  difference  of  the  “2”  coordinates. 
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Example  D 


INPUT  Same  as  Example  A  but  with 

OUTPUT 

to 

II  II 

Cost  =  5.5625e-h004 
Ari  =  -1.7678e-h002 
AV’i  =  -l.OOOOe-002 
AOi  =  -1.4142e-003 
Ars  =  2.8284e-h002 
^V’2  =  -2.0000e-003 
A92  =  -1.4142e-003 
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3  AN  OPTIMIZED  REDUCED-STATE  FILTER  FOR  UNKNOWN  BIAS 


Mookerjee  and  Reifler  [7]  present  a  novel  technique  for  calculating  a  steady-state 
reduced-order  filter  to  track  a  maneuvering  target.  The  filter  they  derive  is  optimized  for 
performance  with  a  stochastic  acceleration.  In  this  chapter,  this  technique  is  modified  to 
derive  a  steady-state  filter  optimized  for  performance  with  a  stochastic  measurement  bias. 
Similar  to  Mookerjee  and  Reifler  [7],  the  filter  developed  in  this  chapter  is  a  reduced-state 
filter. 

The  principles  of  a  reduced-state  filter  applied  to  bias  estimation  can  be  understood  by 
considering  [8]  and  [9].  In  these  reports,  the  position  and  velocity  of  an  aircraft  (a  Beechcraft 
1900)  with  DMEs  [distance  measuring  equipment],  an  INS  [inertial  navigation  system],  and  a 
barometric  altimeter  are  estimated.  The  filter  (in  [8])  and  the  smoother  (in  [9])  were  designed 
with  a  state-to-estimate  range  bias  in  each  DME  (up  to  5  were  used),  a  state-to-estimate  INS 
drift,  and  a  state-to-estimate  bias  in  the  barometric  altimeter.  The  filter  (or  smoother)  ran 
with  these  additional  bias  states  in  tow  (i.e.,  in  addition  to  the  position  and  velocity  states). 
(The  results  in  [8]  and  [9]  achieved  the  design  goals  in  position  and  velocity  accuracy.) 

We  use  discrete  time  dynamical  equations  in  this  report.  It  is  fair  to  consider  the  state  and 
output  (dynamical)  equations  to  be  the  dual,  in  the  control  theory  sense,  of  the  state  and 
input  equations  of  [7].  These  are  (8)  and  (5)  of  [7].  Compared  to  the  dynamical  equations  in 
Mookerjee  and  Reifler  [7],  we  eliminate  the  unknown  acceleration  from  the  state  equation  and 
add  an  unknown  bias  in  the  output  (measurement)  equation,  the  typical  dual  situation.  We 
have: 


X  {k  +  1)  =  ^  {k  +  l,k)  X  {k)  +  B  (fc)  m  {k)  (3-1) 

z{k)  =  H  {k)  ■  x{k)  +  v{k)  +  W  (fc)  u  {x  (k) ,  A)  .  (3-2) 

The  state  x{k)  at  time  k  is  of  dimension  n.  The  state  transition  matrix^  <I>  {I,  k),  of  dimension 
n  by  n,  propagates  the  state  in  time  from  A:  to  Hn  the  absence  of  noise.  The  noise  input 
matrix  B  {k)  is  of  dimension  n  by  h.  The  output  z{k)  at  time  k  is  of  dimension  q  and  the 
output  matrix  H  (k)  is  of  dimension  q  by  n.  The  process  noise  term  m{k)  is  of  dimension  b 
with  covariance  Q  (k).  In  the  sequel,  x  represents  positions  and  velocities,  m  represents 
accelerations,  and  B  (k)  is  an  adjustment  matrix  between  position,  velocity  and  acceleration. 
The  measurement  noise  term  v{k)  is  of  dimension  q  with  covariance  R{k).  The  bias  matrix 
W  (k)  is  q  by  r.  The  bias  function  u  is  51?"'  x  5?^’  — >  5?^,  and  we  have  that  the  bias  A  is  a 
p-dimensional  random  vector  with  mean  A  and  covariance  A. 

The  time  update  equation,  using  (3-1),  is  simply 

X  {k  +  l\k)  =  ^  {k  +  l,k)x  {k\k)  .  (3-3) 

The  measurement  update  equation  becomes 

X  {k  +  l\k  +  1)  =  X  {k  +  Ij/c)  +  K  {k  +  1)  {z  {k  +  1) 

-H{k  +  l)x{k  +  l\k)-W{k  +  l)u{xik  +  l\k),J)}  ,  (3-4) 

'*Bar-Shalom,  Rong  Li,  and  Kirubarajan  [2]. 
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where  K  (k)  is  the  nhy  q  measurement,  or  Kalman,  gain  matrix.  In  the  steady-state  case, 
which  is  discussed  below,  the  position  gain  a  and  velocity  gain  /3  substitute  for  K  (k). 

Filter  Development  —  General  Case 


In  this  section,  we  develop  the  filter  equations  for  the  general  case.  The  development  in 
this  chapter  is  (basically)  dual  (dual  in  the  sense  of  control  theory)  to  Section  III  in  [7] .  The 
error  is  defined  as  (we  develop  the  errors  analogous  to  (27)  and  (32)  of  [7]): 

e  {k  +  l\k  +  1)  =  X  {k  +  1)  —  X  {k  +  l\k  +  1)  (3-5) 


=  X  {k  +  1)  —  X  {k  +  l\k)  —  K  {k  +  1)  {z  {k  +  1)  —  H  {k  +  1)  X  {k  +  l\k) 
—W  {k  +  l)u  (x  {k  +  l|fc) ,  A)) 

=  X  (A;  -|-  1)  —  a;  (/c  -|-  l|/c) 

-K  {k  +  1)  {H  {k  +  l)x  {k  +  1)  +  V  {k  +  1)  +  W  {k  +  l)u  {x  (fc  +  1) ,  A) 
-H  {k  +  l)x  {k  +  l\k)  -  W  {k  +  1)  u  {x  {k  +  l\k)  ,X)}  . 


Continuing, 

e  {k  +  l\k  +  1) 

=  X  {k  +  1)  —  K  {k  +  1)  H  {k  +  1)  X  {k  +  1)  —  K  {k  +  1)  V  {k  +  1) 

-K  {k  +  l)W{k  +  l)u  {x  (k  +  l),  A) 

—X  {k  +  1|A:)  +  K  {k  +  1)  H  {k  +  '^)x  {k  +  '^\k)  +  K  {k  +  '\X}W  {k  +  u  {x  {k  +  \\k) ,  A) 

=  ^  {k  +  l,k)  X  {k)  +  B  {k)  m{k)  —  K  {k  +  1)  H  {k  +  1)  ($  {k  +  l,k)x  {k)  +  B  {k)m  (k)) 
-K  {k  +  1)  V  {k  +  1)  -  K  {k  +  l)W  {k  +  1)  u  ($  {k  +  l,k)x  (k)  +B{k)m  {k) ,  A) 

—  {k  +  l,k)x  {k\k)  +  K  {k  +  1)  H  {k  +  1)  ^  {k  +  1,  k)  X  {k\k) 

+K  {k  +  1)W  {k  +  l)u  (<I>  {k  -|-  1,  A:)  X  (A:|A:) ,  A) 

=  {I  -  K{k  +  l)H{k  +  l))^{k  +  1,  k)  (x  (A:)  -  X  (A:|A:)) 

+  {I  -  K{k  +  l)H{k  +  l))B  (k)  m{k)-K{k  +  l)v{k  +  1) 

—K  {k  +  1)W  {k  +  1)  {u  (^h  {k  +  l,k)x  (k)  +  B  {k)m  (k) ,  X)  —  u  {k  +  I,  k)  x  {k\k) ,  A)}  . 


So 

e  {k  +  l\k  -|-  1)  =  L  (A:  -|-  1)  (A;  -|-  1,  A:)  e  (A:|A;) 

+L  {k  +  1)B  (k)  m{k)  -  K  {k  +  1)  {W  {k  +  1)  -|-  u  (A:  -|-  1))  ;  (3-6) 

where 

L{k)  =  {I  -  K{k)H{k))  (3-7) 


an  n  by  n  matrix,  and 


Aufc_|_i|fc_|_i  =  u  (<I>  (A:  -|-  1,  A:)  X  {k)  -|-  i?  (A:)  m  {k) ,  A)  —  u  (<I>  (A:  -|-  1,  A:)  x  (A:|A:) ,  A)  .  (3-8) 
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We  make  the  linear  approximation 


An 


'fc+l|A:+l 


du 

dx 


ai=^(/i;+l|/c+l),A=A 


du 


(<i>  {k  +  1,  k)  Ax  +  B  {k)m  (k)) 


AA 


where 

and 


a:=^(/c+l|fcH-l),A=A 
Ax  =  e  {k\k)  =  x{k)  —  X  {k\k)  , 
AA=A-A . 


(3-9) 

(3-10) 

(3-11) 


The  result  obtained  is 

s  {k  +  l\k  +  1)  =  L  {k  +  1)  ^  {k  +  1,  k)  s  {k\k)  +  L  {k  +  1)  B  (k)  m  (k) 

-K{k  +  l)W{k  +  l) 


du 

dx 


x=x{k-\-l\k+l)  ^X=X 


f)qi 

(<h  {k  +  l,k)£  {k\k)  +  B{k)m  (k))  +  — 


AA 


iE=x(/c+l|/c+l),A=A 


—K  {k  +  l)v  {k  +  1) 


=  [L{k  +  l)-K{k  +  l)W{k  +  l)  — 


x=x(k+l\k+l)  ^X=X  ^ 


^  {k  +  l,k)  e  {k\k) 


f)li 

-K{k  +  l)W{k  +  l)  ^ 


AA 


x=x{k-\-l\k-\-l),X=X 


(  3l! 

+  [L{k  +  l)-K{k  +  l)W{k  +  l)  — 


Define 


x=x(k-\-l\k-\-l)  ,X=X  ^ 


B  (k)  m{k)  —  K  {k  +  l)v  {k  +  1)  . 

(3-12) 


F  {k  +  l,k)  =  il  {k  +  1)  -  K  {k  +  1)W  {k  +  1)  — 


x=x(k+l\k-\-l)  ,X=X  y 


an  n  by  n  matrix,  and 


an  n  by  p  matrix.  Set 


3ij 

C  (k)  =  -K  {k)W  (k)  — 


x=x{k\k),X=X 


m  (fc)  =  ^  {k  +  l,k)  B  (k)  m  (fc) 

and  take  note  that  the  covariance  of  fh  is 


^{k  +  l,k)  ,  (3-13) 

(3-14) 

(3-15) 


E[m  {k)  m  (A:)']  =  Q  (k)  =  E  {k  +  l,k)B  {k)  m  (k)  m  (fe)'  B  (fe)'  ($-^  (k  +  1,  k)) 
=  {k  +  l,k)B  (k)  Q  (k)  B  {k)'  ($-^  {k  +  1,  k))'  . 


(3-16) 
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Then  (3-12)  becomes 

e  {k  +  l\k  +  1)  =  F  {k  +  1,  k)  e  {k\k)  +  F  {k  +  l^k)  m  {k)  +  C  {k  +  1)  AA  —  K  {k  +  l)v  {k  +  1)  . 

(3-17) 

We  now  implement  the  observation  made  in  [7]  that  the  error  e{k\k)  may  be  viewed  as 
consisting  of  two  components.  The  first  component  of  error,  is  due  to  the  (unbiased) 
process  noise  m,  (3-15),  and  the  measurement  noise  v.  The  second  component  of  error,  is 
due  to  the  measurement  bias.  To  the  extent  that  the  linear  approximation  is  valid,  a  linear 
analysis  holds.  That  is,  the  two  error  inputs  may  be  treated  in  separate  equations  by  applying 
the  superposition  principle  of  linear  analysis.  Applying  the  superposition  principle  to  (3-17) 
we  get  these  two  equations: 


di) 


{k  +  l\k  +  l)  =  F{k  +  1,  k)  {k\k)  +  F  {k +  l,k)m{k)  -  K  {k  +  l)v  {k  +  1)  (3-18) 


{k  +  l\k  +  l)  =  F{k  +  l,k)  {k\k)  +  C  (fe  +  1)  •  AA  . 


(2) 


(3-19) 


These  equations  are  comparable  to  (33)  and  (24)  of  [7].  Equation  (3-18)  contains  the 
unbiased  noise  and  (3-19)  contains  the  bias. 

In  addition,  we  require  update  equations  for  the  total  covariance  and  the  covariance  of 
{k\k).  Define 

M  {k  +  l\k)  =  E  [eW  {k  +  l\k)  {k  +  l\k)' 


and 


(3-20) 

(3-21) 


M{k  +  l\k  +  l)  =  E  {k  +  l\k  +  1)  {k  +  l\k  +  1)' 

Substituting  (3-1)  into  (3-20)  we  get  that  the  time  updated  covariance  for  is 

M  {k  +  l\k)  =  E[x{k  +  \)  —  x{k  +  l|/c)]  [x{k  +  l)  —  x{k  +  1|A:)]' 

=  E[^  {k  +  l^k)  X  (k)  +  B  {k)m  {k)  —  ^  {k  +  l,k)x  {k\k)] 

[<h  {k  +  l,k)x  {k)  +  B  {k)m  {k)  —  ^  {k  +  l,k)x  {k\k)]' 

=  E  [<h  (fc  -h  1,  k)  (x  (k)  —  X  {k\k))  +  B  {k)m  (A:)]  [$  {k  -|-  1,  k)  (x  {k)  —  x  ik\k))  +  B  {k)m  (k)]' 
=  ^{k  +  l,k)M  {k\k)  $  (A:  +  1,  k)'  +  B{k)Q  (k)  B  (k)'  .  (3-22) 

Substituting  (3-18)  into  (3-21)  measurement  updated  covariance  for  is 

M  (A:  +  1|A:  +  1)  =  E  (A:  +  1|A:  +  1)  {k  +  1|A:  +  1)' 

=  E  (^E{k  +  1,  k)  (A:|A:)  +  F  {k  +  l,k)  in  {k)  -  K  {k  +  1)  v  {k  +  1)) 

(f  {k  +  1,  k)  (A:|A:)  +  F  (A:  +  1,  A:)  m  (A:)  -  (A:  +  1)  n  (A:  +  1))' 

and  we  use  E  [n  {k)  v  (Z)^]  =  0  for  A:  /  1  giving 
F  [F  (A:  +  1,  k)  (A:|A:)  (FT  (A:  +  1)  n  (A:  +  1))']  =  0.  Hence, 


M  {k  +  l\k  +  l)  =  E  E{k  +  1,  k)  (A:|A:)  (A:|A:)'  F  (A:  +  1,  k)' 


AT 
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+E  [F  {k  +  l,k)m  {k)  m  (k)'  F  {k  +  1,  k)'  +  K  {k  +  1)  v  {k  +  l)v  {k  +  1)'  K  {k  +  1)'] 

=  F{k  +  l,k)M  {k\k)  F{k  +  l,  k)'  +  F{k  +  l,k)Q  (fc)  F  (fc  +  1,  k)' 

+K{k  +  l)R{k  +  l)K{k  +  l)'  .  (3-23) 

Our  goal  is  to  formulate  the  update  equations  for  the  total  covariance,  so  we  define  the  n  by  p 
matrices  D  {k\k)  and  D  {k  +  1|A:),  and  then  note 

(A:|fc)  =  Z)(fc|fe)  •  AA  .  (3-24) 

In  view  of  our  linearized  analysis,  we  can  define  D  {k\k)  in  this  way  because  the  system  output 
(e^^)  {k\k))  is  a  linear  function  of  the  system  input  (AA).  Proceeding,  D  {k  +  l\k)  is  defined  as 

D{k  +  l\k)  =  F{k  +  l,k)D{k\k)  .  (3-25) 

In  (3-24),  and  AA  are  known  quantities  [the  equation  defines  D  {k\k)].  In  (3-25),  F  (k) 
and  D  {k\k)  are  known  quantities.  Then,  substituting  (3-24)  into  (3-19),  we  obtain 

F  (A:  +  1|A:  +  1)  •  AA  =  F  (A:  +  1,  ^)  F  (A:|A:)  •  AA  +  C  (A:  +  1)  •  AA 

=  F(A:  +  1|A:)-AA  +  (:7(A:  +  1)-AA  ,  (3-26) 

and  consequently  (since  (3-26)  holds  for  all  AA) 

F  (A:  +  1|A:  +  1)  =  F  (A;  +  l|fc)  +  C  (A:  +  1)  .  (3-27) 


Let  S  be  the  total  error  (due  to  m,  v,  and  A)  covariance.  By  superposition,  we  get  the 
total  error  by  the  addition  of  the  two  error  terms.  We  observe  that  since  the  two  errors, 
and  originate  from  independent  sources,  they  remain  independent  for  all  times  k.  By 
considering  the  definition  of  S  (below),  we  observe  that 

S{k  +  l\k)  =  F[£{k  +  l\k)e{k  +  1|A:)'] 


=  F 

=  F 


{k  +  1|A;)  -|-  {k  +  1|A:)^  {k  -|-  1|A:)  -|-  {k  + 


+  F 


{k  +  1|A:)  {k  +  l\ky 


(2) 


{k  +  1|A;)  {k  +  1|A;)' 

=  M{k  +  l\k)  +  F  [e(2)  {k  +  l\k)  {k  +  l\k)' 

=  M{k  +  l\k)+F  [4>  {k  +  l,k)D  {k\k)  AA  •  AA'F  {k\k)'  $  (fc  +  1,  A:)']  , 
using  (3-1)^,  (3-3),  and  (3-24).  Hence, 

5  (A:  +  1|A:)  =  M  (A:  +  1|A:)  +  $  (A:  +  1,  A:)  F  {k\k)  F  [AAAA']  F  (A:|fc)'  $  (fc  +  1,  k)'  . 

Finally,  we  note 

5  (fc  +  l|fc)  =  M  (A:  +  1|A:)  +  $  (A:  +  1,  A:)  F  (fc|A:)  AD  {k\k)'  <^{k  +  l,  k)'  .  (3-28) 


®The  process  noise  part  of  (3-1)  does  not  figure  into 
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Basically,  this  is  the  same  result  found  in  (19)  of  [7]. 

We  next  obtain  the  measurement  update  for  S: 

S{k  +  l\k  +  l)  =  E[e{k  +  l\k  +  l)e{k  +  l\k  +  1)'] 

=  E  [{x  {k  +  1)  —  X  {k  +  l\k  +  1))  {x{k  +  1)  —  x{k  +  l\k  +  1))'] 

=  E  [{x  (k  +  1)  —  X  (k  +  l\k) 

-K  (fc  +  l){z{k  +  l)  -  H{k  +  l)xik  +  l\k)  -W{k  +  l)u  {x  (fc  +  l|fc) ,  A)) } 

{ditto}^]  , 

using  (3-4), 

=  E  [{x  {k  +  1)  —  X  {k  +  l|fe) 

-K  {k  +  l){H{k  +  l)x{k  +  l)  +  v{k  +  l)  +  W{k  +  l)u  {x  {k  +  l),  A) 

—H  {k  +  l)x  {k  +  l\k)  —  W  {k  +  1)  u  (x  (A:  -|-  1| fe) ,  A)) }  {ditto}^]  , 

using  (3-2), 

=  E  [{x  {k  +  1)  —  X  {k  +  l\k)  —  K  {k  +  1)  H  {k  +  1)  {x  {k  +  1)  —  x  {k  +  1|A:)) 

—K  {k  +  1)  [v  {k  +  1)  +  W  {k  +  1)  u{x  {k  +  1) ,  X)  —  W  {k  +  1)  u  (x  (A:  -|-  1|  A:) ,  A)) }  {ditto}'] 

=  E  [{x  {k  +  1)  —  X  {k  +  l\k)  —  K  {k  +  1)  H  {k  +  1)  (x  (A:  -|-  1)  —  a:  (A:  -|-  1|A:)) 

—K  {k  -|-  1)  (n  (A:  -|-  1)  -|-  W  (A;  -|-  1)  [u  {x  {k  +  1) ,  X)  —  u  {x  {k  +  l\k) ,  X)))}  {ditto}'] 

=  ^  [{x  (A:  -|-  1)  —  X  (A:  -|-  1|A:)  —  iiT  (A:  -|-  1)  fA  (A:  -|-  1)  (x  (A;  -|-  1)  —  x  (A:  -|-  1|A:)) 

—K  (k)  [v  {k  +  1)  +  W  {k  +  1)  {u  (4>  {k  +  l,k)x  (k)  +  m  (k) ,  X)  —  u  {k  +  1,  k)  x  {k\k) ,  A)))} 

{ditto}'] 

=  £■  [{x  (A:  -|-  1)  —  X  (A:  -|-  1|A:)  —  iiT  (A:  -|-  1)  (A:  -|-  1)  (x  (A;  -|-  1)  —  x  (A:  -|-  1|A:)) 

-K  {k  +  1)  {v{k  +  l) +  W  {k  +  1)  (Aufc+iifc+i)) }  {ditto}'  ]  , 

using  (3-8). 

We  take  this  next  step  only  to  the  extent  of  the  approximation. 


S{k  +  l\k  +  l)  =  E[{{I-K{k  +  l)H{k  +  l))ix{k  +  l)-x{k  +  l\k)) 
-K  {k  +  l){v{k  +  l)  +  W  {k  +  1) 

{ditto}']  , 


($  (A:  +  1,  k)  e  {k\k)  +  m  {k))  +  AA 


using  (3-9),  where  we  have  omitted  the  substitution  limits  on  the  partial  derivative  fractions. 
Continuing, 

=  E[{{I -K{k  +  l)H{k  +  l))e{k  +  l\k) 


-K  {k  +  l)(^vik  +  l)  +  W{k  +  1)  (k  +  l\k)  +  I^AA 


{ditto}'] 
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=  E 


3ii  \ 

I-K{k  +  l)H{k  +  l)-K(k  +  l)W(k  +  l)  —  ]e{k  +  l\k) 

ox  J 

-K  {k  +  l){v{k  +  l)  +  W{k  +  1)  I^AA 
{ditto}^]  . 


Let 


and 


~  (9?/ 

H  {k)  =  H  {k)  +  W  {k)  — 

_ _ _  f^rt  I  p^r>  I  ^ 

R{k)  =  R  {k)  +  VL  {k)  —A—  W  (k)'  . 

Therefore,  we  have 

S  (k  +  Hk  +  1)  =  (^I  -  K  (k  +  1)  ff  (k  +  1))  S(k  +  l\k)  (^I  -  K  {k  +  1)  H  {k  +  I 

+K  {k  +  1)  R{k  +  1)  K  {k  +  1)' 

-(^I-K{k  +  l)H{k  +  l)')E  [e  (k  +  l\k)  AA']  (w  {k  +  1)  K{k  +  1)' 

-K  {k  +  1)  {w  {k  +  1)^^E  [AXe  {k  +  l\ky]  (^I  -  K  {k  +  1)  H  (k  +  1)')'  . 
Combining  (3-1)  and  (3-3)  and  substituting  into  (3-24)  yields 

E[eik  +  l\k)  AA']  =  E  \ (k  +  l\k)  +  (k  +  1| A:))  AA' 

=  E 


e(2)  (k  +  1|A:)  AA'l  =^{k  +  l,k)E  [e^^)  (^k\k)  AA' 


=  <^>ik  +  l,k)E[D  {k\k)  AAAA'j  =<^{k  +  l,k)D  {k\k)  A  . 


Hence, 


S  {k  +  l\k  +  1)  =  [I  -  K  {k  +  1)  H  {k  +  l)j  S  {k  +  l\k)  -  K  {k  +  1)  H  {k  +  I 

+K  {k  +  1)  R{k  +  1)  K  {k  +  1)' 

du\' 


(3-29) 

(3-30) 


-  (^I  -  a:  (A:  +  1)  i/  (fc  +  l)j  d>  (A:  +  1,  k)  D  {k\k)  Ai^W{k  +  1)  — J  K  {k  +  1)' 

-K  {k  +  1)  {w  {k  +  1)  AD  {k\ky  $  (A:  +  1,  k)'  (^I  -  K  {k  +  1)  H  {k  +  1)^'  .  (3-31) 

Equation  (3-31)  is  similar  in  form  to  (37)  of  [7].  The  completing-the-square  technique  may  be 
used  to  solve  for  iL  (A:  -|-  1)  in  this  equation.  First,  we  expand  (3-31)  as 

5  (A:  +  1|A:  +  1)  =  5  (A:  +  1|A:)  +  K  {k  +  1)  H  {k  +  1)  S  {k  +  l\k)  H  {k  +  1)'  K  {k  +  1)' 

+A:(A:  +  1)R(A:  +  1)A:(A;  +  1)' 
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du 


+K  {k  +  l)H  {k  +  l)<^ik  +  1,  k)  D  {k\k)  A{W{k  +  l)  —  ]  K{k  +  iy 


du 


+K  {k  +  1)  {k  +  1)  —  J  AD  {k\k)'  $  (A:  +  1,  k)'  H{k  +  1)'  K{k  +  1)' 
-^>  {k  +  l,k)D  {k\k)  A(w{k  +  1)  K{k  +  1)' 

-K  {k  +  1)  {w  {k  +  1)  ^  AD  {k\ky  $  (A:  +  1,  k)' 

-K  {k  +  1)  H  {k  +  1)  S  {k  +  l\k)  -  S  {k  +  l\k)  H  {k  +  ly  K  {k  +  ly  . 
Next,  gather  the  like  terms: 

5(A:  +  1|A:  +  1)  =  5(A;  +  1|A;) 

+K  {k  +  1)  [h  {k  +  l)S{k  +  1| A:)  H{k  +  iy  +  R{k  +  1) 

—  /  f)!!  \  ^ 

+H  (A:  +  1)  $  (A:  +  1,  k)  D  {k\k)  A  f  (A:  +  1)  —  j 
+  {w{k  +  1)  ^  AD  {k\ky  (A:  +  1,  k)'  H{k  +  1)'^  K  {k  +  1)' 

-K  (A:  +  1)  (w  {k  +  1)  ^  AD  (A:|  A:)'  (A:  +  1,  k)'  +  H  {k  +  1)  S  {k  +  l\k) 

-  (^{k  +  l,k)D{k\k)A(w{k  +  l)^^  +S{k  +  l\k)H{k  +  iy']  K{k  +  iy  . 


To  condense  the  notation,  define 

X{k  +  l)=(wik  +  1)  AD  {k\ky  ^{k  +  1,  k)' 


Y  {k  +  1)  =  H  {k  +  1)  S  {k  +  l\k)  H  {k  +  1)'  +  R{k  +  1) 

du' 


+H  (A:  +  1)  $  (A:  +  1,  k)  D  {k\k)  A  {  W  {k  +  1) 


dX 


du 


(3-32) 


+  [W{k  +  1)  —  J  AD  {k\ky  $  (A:  +  1,  k)'  H{k  +  1)' 

(^H  {k  +  l)S{k  +  l\k)  H  {k  +  1)'  +  R{k  +  1)  +  H  {k  +  1)  X  {k  +  ly  +  X  {k  +  1)  H  {k  +  1)') 


and 


Z{k  +  1)  =  (^X{k  +  1)'  +  S{k  +  l\k)  H{k  +  1)')  y  (A:  +  1)"^  . 


We  have  that  X  (k)  is  a  g  by  n  matrix,  Y  (k)  is  a  g  by  g  matrix,  and  Z  (k)  is  an  n  by  g 
matrix.  Then  (3-32)  becomes 

5  (A:  +  1|A:  +  1)  =  5  (A:  +  1|A:)  +  K  {k  +  1)Y  {k  +  1)  K  {k  +  1)' 

-K  {k  +  1)  (x  {k  +  1)  +  H  {k  +  1)  S  {k  +  l\k))-(x  {k  +  1)'  +  S{k  +  l\k)  H  {k  +  1)')  K  {k  +  1)' 
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=  S{k  +  l\k)  +K{k  +  l)Y{k  +  l)K{k  +  1)' 

-K  {k  +  l)Y{k  +  1)  {k  +  l)(^X{k  +  l)+H{k  +  l)S{k  +  l|fc)) 

-(^X{k  +  1)'  +  S{k  +  l\k)H{k  +  1)')  y-i  {k  +  1)Y  {k  +  1)  K  {k  +  1)' 

=  S  {k  +  l\k)  +  K  {k  +  1)Y  {k  +  1)  K  {k  +  1)'  -  K  {k  +  1)Y  {k  +  1)  Z  {k  +  1)' 

-Z  {k  +  1)Y  {k  +  1)  K  {k  +  1)' 

=  S  {k  +  l\k)  +  K  {k  +  1)Y  {k  +  1)  K  {k  +  ly  -  K  {k  +  1)Y  {k  +  1)  Z  {k  +  1)' 

-Z  {k  +  1)Y  {k  +  I)  K  {k  +  1)' 

+Z  {k  +  l)Y{k  +  l)Z{k  +  1)'  -  Z  {k  +  1)Y  {k  +  1)  Z  {k  +  1)' 

=  S{k  +  l\k)  +  {K{k  +  l)-Z{k  +  1))  Y{k  +  1)  {K  (k  +  1)'  -Z{k  +  1)') 

-Z{k  +  l)Y{k  +  l)Z{k  +  iy  .  (3-33) 

We  see  that  (3-33)  is  minimized  by  setting  K  (k  +  1)  =  Z  (k  +  1).  The  optimal  filter  gain  (i.e., 
optimal  Kalman  gain  matrix)  is 


K{k  +  l)  =  Z{k  +  l)=  (s{k  +  l\k)H{k  +  1)'  +  ^{k  +  l,k)D  {k\k)  A  f  IT  (A:  +  1) 


X  (H{k  +  l)S{k  +  l\k)H{k  +  iy  +  R{k  +  l) 


du 


+H  {k  +  l)<^{k  +  1,  k)  D{k\k)A{W{k  +  l)  — 


du 


+  (  W(A:  +  1)  —  )  ADik\ky<^ik  +  l,kyHik  +  iy 


-1 


which  is  an  n  by  g  matrix.  The  form  used  below  is 

K{k  +  1)  (h  {k  +  l)S{k  +  1|A:)  H  {k  +  iy  +  R{k  +  1) 


+H  (k  +  l)^(k  +  1,  k)  D(k\k)A(W(k  +  l)  — 

\  oX 

+  (w{k  +  1)  AD  {k\ky  ^{k  +  l,  k)'  H{k  +  1)'^ 

~  f  dv\' 

=  S{k  +  1|A:)  H{k  +  iy  +  ^{k  +  1,  k)  D  {k\k)  A  Iw  {k  +  1)  —  ] 


(3-34) 


(3-35) 


Filter  Development — Steady-State  Case 


We  now  examine  the  steady-state  case  of  our  problem.  Referencing  equations  (41-43)  of  [7] 
we  have 


= 


1  T 
0  1 


$-1  = 


1  -T 
0  1 


(3-36) 
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where  T  is  a  step  size  in  seconds, 

ff=[  1  0] 


and 


W  =  1  . 


There  are  three  cases  for  B.  First,  let 


5a  = 


0 

1 


(3-37) 

(3-38) 


(3-39) 


We  have  that  m  (k)  is  a  velocity  noise  in  this  case.  In  the  other  two  cases,  m  (k)  is  an 
acceleration  noise.  In  these  cases 

Bb=\  ^ 


(3-40) 


and 


(3-41) 


In  (3-39),  process  noise  enters  the  system  by  the  velocity  state  as  velocity.  In  (3-40),  process 
noise  enters  the  system  by  the  velocity  state  as  acceleration  multiplied  by  time.  This  noise 
affects  the  position  state  by  way  of  the  integration  in  the  dynamics.  This  is  similar  to  the 
set-up  in  Benedict  and  Bordner  [3].  In  (3-41),  process  noise  enters  the  system  as  acceleration 
in  the  position  state  and  velocity  state.  This  arrangement  is  similar  to  the  set-up  in  Kalata  [6] 


Case  a:  Ba  =[  0  1 


We  treat  the  first  case  then,  subsequently,  appropriately  modify  various  equations  to  adapt 
to  the  other  two  cases.  Hence,  considering  B  as  in  (3-39),  that  is  5  =  B^,  with  p  as  position, 
V  as  velocity,  and  z  as  the  measurement,  the  state  transition  and  output  equations  for  the 
steady-state  case  are 


p{k  +  l) 
V  {k  +  1) 


1  T 
0  1 


p{k) 
V  (k) 


+ 


0 

1 


m  {k) 


z{k)  =[  I  0 


p{k) 

V  (fc) 


+  V  (k)  +  u  {x  (k) ,  X{k))  . 


Setting  u  (x,  A)  =  A,  the  linear  approximations  from  (3-9)  become 


du 

dx 


x=x(k\k)^X=X 


=  [0  0] 


and 


du 


=  1  . 


x=x{k\k),X=X 


Then,  substituting  (3-44)  into  (3-29) 


du 


^  =  ^  +  0]+l.[0  0]  =  [1  0]=H 


(3-42) 

(3-43) 

(3-44) 

(3-45) 

(3-46) 
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and  substituting  (3-45)  into  (3-30) 


3vj  Oil  ^ 

R  =  R+  VF  — Attt  W'  =  R+l-l-A-l-l  =  R  +  A. 
oX  oX 


(3-47) 


Considering  (3-15), 

fh  =  ^~^Bm  = 
From  (3-16)  and  (3-39)  we  have  that 
Q  =  («F~^)'  = 

The  steady-state  filter  gain  is 


1  -T 
0  1 


0 

1 


m  = 


-T 

1 


m 


■  1  -T  ■ 

■  0  ■ 

Q[  0  1  ] 

■  1  O' 

■  r2  -r  ■ 

0  1 

1 

-T  1 

— 

-T  1 

Q  .  (3-48) 


K  = 


a 

P/T 


(3-49) 


As  mentioned,  (3-49)  is  where  a  and  /?  fit  in  for  the  Kalman  gain  matrix  K  (k)  given  by 
(3-34).  These  gains  are  obtained  by  computing  the  steady-state  values  for  all  variables  in 
(3-34).  The  objective  of  this  section  is  to  find  a  relationship  between  a  and  /3. 

The  steady-state  version  of  L  (k)  from  (3-7),  L,  is 


1  0 
0  1 


a 

P/T 


L  (k)  =  {I-  KH)  = 

The  steady-state  version  of  F  {k)  from  (3-13),  T,  is 


[1  0]  = 


1  —  a  0 
-P/T  1 


(3-50) 


F  = 


1  —  a  0 
-P/T  1 


a 


HIT 


l.[0  0] 


■  1 

T  ■ 

0 

1 

1  —  a  {1  —  a)T 
-P/T  1  -  (5 


The  eigenvalues  of  F  are 


ei,2  = 


2  2 

Then,  referring  to  (3-14),  the  steady-state  version  of  C  (k),  C,  is 


C  =  - 


a 

P/T 


.1.1  =  - 


a 

P/T 


The  measurement  updated  steady-state  covariance  M  {k\k),  referring  to  (3-23),  is 

M=  lim  M  {k\k)  =  FMF' +  FQF' +  K  RK'  . 

k^oo 

Using  the  superposition  principle  by  letting 

M  =  Mq  +  AIr 


(3-51) 


(3-52) 


(3-53) 


(3-54) 


(3-55) 
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and  then  solving 

M  R  =  FM  rF' +  K  RK'  (3-56) 

Mq  =  FMqF'  +  FQF'  .  (3-57) 

Comparing  (3-49),  (3-51),  and  (3-56)  to  (46),  (47),  and  (48)  of  [7],  we  see  that  our  solution  for 
Mr  is  of  the  same  form  as  (49)  of  [7].  Consequently, 


Mr 


R 

a  {A  —  2a  —  (3) 


2a2  +  2/3-3a/3  /3  (2a  - /3) /T 

P{2a-p)/T  2/3Vr2 


(3-58) 


The  solution  of  Mq  remains  to  be  determined.  From  (3-48)  and  (3-51), 


fqf'  = 

1  —  a  (1  —  a)  r 
.  -P/T  1-/3 

rjp2 

-T 

-T  ■ 
1 

■Q- 

1  —  a  (1  —  a)  T 
.  -NT  1-/3 

= 

■  0  0  ■ 
0  1 

■Q. 

Substituting  (3-51)  and  (3-59)  into  (3-57)  gives 


Mq  = 


1  —  a  (1  —  a)  T 
-P/T  1  -  P 


Mq 


1  -  a  -P/T 
(l-a)r  1-/3 


0  0 
0  1 


■Q. 


In  longhand. 


Mq  = 


miiQ 

mi2Q 


mi2Q 

Fi22Q 


(1  -  a)^  {muQ  -h  2Tmi2Q  -h  T‘^m22Q) 

(1  -  a)  {-PmiiQ/T  (1  -  2/3)  mi2Q  +  T{1-  P)  m22Q) 


(1  -  a)  {-Pmuq/T  -h  (1  -  2/3)  mi2Q  +  T{1-  P)  m22Q) 
{P^/T"^)  miiQ  +  (2/3  (/3  -  1)  /T)  mi2Q  +  (l  -  2/3  +  P^)  m22Q 


+ 


0  0 
0  1 


■Q. 


Hence, 


miiQ  -  (1  -  af  (miiQ  -F  2Tmi2Q  +  T'^m22Q) 

_  mi2Q  -  (1  -  a)  {-Ptuhq/T  -h  (1  -  2P)  mi2Q  +  T{1-  P)  m22Q) 

mi2Q  -  (1  -  a)  {-PmiiQ/T  -h  (1  -  2/3)  mi2Q  +  T{1-  P)  fn22Q) 
m22Q  -  {P'^/T'^)  miiQ  -  (2/3  {P  -  1)  /T)  mi2Q  -  {l  -  2P  +  /3^)  m22Q 


0  0 
0  1 


■Q. 


(3-59) 


(3-60) 


(3-61) 


(3-62) 


We  get  three  equations  in  the  three  unknowns  muQ,  uiuq,  and  7n22Q  (defining  A  by  the 
3-by-3  matrix  on  the  right  hand  side). 


miiQ 

■  1  -  (1  -  a)^ 

-2(1  -  afT 

-(l-a)^r2 

miiQ 

A 

mi2Q 

= 

(l-a)/3/r 

1  -  (1  -  a)  (1  -  2/3) 

-(l-a)(l-/3)r 

mi2Q 

_  FI22Q 

L 

-2/3  (/3-i)/r 

1  -  (1  -  2/3  +  ^2) 

_  FI22Q 

3-12 


NSWCDD/TR-12/555 


0 

0 


Q 


(3-63) 


where  (3-63)  also  defines  the  3-by-3  matrix  A.  Taking  the  matrix  inverse  of  A  to  solve  for  Mq, 


rniiQ 

■  1  -  (1  -  a)^ 

-2(1  -  afr 

-(l-a)^r2 

-1 

■  0  ■ 

muQ 

= 

(l-a)/3/r 

l-(l-a)(l-2/3)  -(l-a)(l-^)r 

0 

rn22Q 

-2/3  (/3-l)/T 

1-{1-2P  +  P^) 

.  Q  . 

The  determinant  of  A  is:  det  (^)  =  4a/3  —  a/3^  —  2a^/3  =  a/3  (4  —  /3  —  2q;);  and  shonld  not  be 
zero  for  the  inverse  to  exist.  This  is  satisfied  by  these  conditions: 


1.  a  7^  0 

2.  ^7^0  .  (3-64) 

3.  /3  7^4-2a 


If  the  determinant  of  A  is  not  zero,  we  can  obtain  the  solntion: 


rnuQ 

(-2  +  5a  -  4a2  +  a^) 

rni2Q 

=  Q- 

T  (—2a  +  13  —  a(3  +  3a^  —  a^) 

_  rn22Q 

{-2(3  +  2al3-2a'^  +  a^) 

/  (-4a/3  +  a/3^  +  2a^/3)  .  (3-65) 


In  matrix  form, 


Mq  = 


Q 


_  (-2  -F  5a  -  4a^  +  a^) 

(— 4a/3  -|-  af3‘^  +  2a^/3)  T  (—2a  +  (3  —  af3  +  3a^  —  a^) 


T  (—2a  +  (3  —  a/3  -|-  3a^  —  a^) 
(-2/3 -k  2a/3  -  2a2 -K  a^) 


And  finally  M  is  obtained  from  (3-55),  (3-58)  and  (3-66): 


M  = 


R 


2a2  +  2^-3a/3  /3  (2a  -  ^) /T 


a  (4  -  2a  -  ^)  [  /3  (2a  -  /3)  /T  2/32/^^ 


(3-66) 


Q 


(— 4a/3  -|-  a/3^  -|-  2a^0) 


T 


(—2  -|-  5a  —  4a^  -|-  a^) 
(—2a  +  (3  —  a(3  +  3a^  —  a^) 


T  (—2a  +  (3  —  aj3  +  3a^  —  a^) 
(-2^  -k  2a(3  -  2o?  -h  a^) 

(3-67) 


We  see  that  mu  =  mu  (a,  /3,  T,  i?,  Q),  mi2  =  mi2  (a,  /3,  T,  i?,  Q)  and  m22 
=  m22  (a,  /3,  r,  i?,  Q).  The  nsnal  techniqne  for  solving  the  Liapnnov  eqnation  (3-54)  is  by 
algebraic  manipnlation  and  nsing  the  symmetry  of  the  matrix,  as  demonstrated  with  the 
solntion  (3-67).  Nnmerical  solntions  may  be  obtained  by  repeated  propagation  nntil  arriving 
at  steady-state.  We  note  that  mwjQ  is  the  so-called  noise  reduction  factor;  see  [2]. 

O 

The  time-npdated  steady-state  covariance  M,  referring  to  (3-22),  is 


A/  =  lim  AI  {k  +  l\k) 
k—^oo 


lim  {k\k)  «4>'  -h  BQB'  =  4>  {Mr  +  Mq)  4>'  -h  BQB'  . 

k—^'OO 


(3-68) 
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We  get 


^Mr^'  = 


R 

'IT' 

■  2a^  +  2(3-  3aP 

/3  (2a  -/3)IT' 

■  1  O' 

2a-  13) 

0  1  _ 

[  P{2a-P)IT 

2/37t2 

T  1 

R 


2a2  +  2/3  +  a/3  /3  (2a  + /3) /T 


and 


a(4-2a-/3)  [  ^  (2a  +  ^) /T 
^Mq^'  = 

Q 


2^7r^ 


■  1 

T  ■ 

0 

1 

1 

0  ■ 

T 

1 

Hence, 


+ 


M  = 

Q 


R 


2a2  +  2/3  +  a/3  /3  (2a  + /3) /T 


a  (4  -  2a  -  ;3)  [  ^  (2a  +  /?)  /T  2/3Vr2 

r7-2  +  a)  T  (-2a  - /3  +  a/3  +  a2) 


(— 4a/3  +  a/3^  +  2a^/3)  (—2a  —  /3  +  a/3  +  a^)  (—2/3  +  2al3  —  20?  +  a^) 

+ 


0  0 
0  Q 


We  need  steady-state  versions  of  these:  designating 


(—2  +  5a  —  4a^  -|-  a^) 

(— 4a/3  -|-  a/3^  -|-  2a^/3)  0  I  T  (—2a  +  /3  —  a/3  -1-  3a^  —  a^) 

T  (—2a  +  13  —  aj3  +  3a^  —  a^) 

(-2/3  -h  2a/3  -  2a2  a^ 

Q  T'^  [—2  +  a)  T  (— 2a  — /3 -|- a/3 -|- a^) 

(— 4a/3  -|-  a/3^  -|-  2a^/3)  T  (—2a  —  ^  a^  -|-  a^)  (—2/3  -|-  2a/3  —  2a^  -|-  a^) 


(3-69) 


D  =  lim  D  {k\k) 

k—^oo 


and 

D  =  lim  D  (k  +  l\k)  , 

fc— >c» 

we  then  have,  referring  to  (3-25)  and  (3-27), 


D  =  FD 


D  =  b  +  C 

then 

D=D-C . 


Continuing, 


hence. 


FD  =  D-C 

FD  -D=(F-I)D  =  -C-, 
D  =  -  {F-I)~^C  . 
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Using  (3-51),  and  (3-53) 
D  = 


( 

1  —  a  (1  —  a)  T 

■  1  0  ■ 

i’7 

a 

.  -PIT  1-/3 

0  1 

)  ( 

PIT 

—a  (1  —  a)  T 
-/3/T  -p 

-P  -{l-a)T 

PIT 


-1 


a 

PIT 


—a 

a 

-1 

P 

[  PIT  \ 

0 

Hence, 


D  =  FD  = 


1  —  a  (1  —  a)  T 

■  -1  ■ 

a  —  1 

_  -PIT  1-/3 

0 

.  PIT  _ 

Finally,  the  steady-state  time-updated  total  (due  to  noise  and  bias)  covariance,  S,  is 
obtained  by  substituting  into  (3-28), 


S  = 

Mil  Mi2 
M21  M22 


5ll  5i2 

O  O 

5'21  S22 


=  M  + 


+ 


1  T 
0  1 


-1 

0 


A.[-l  0] 


1  0 
T  1 


Mil 

1 

(M 

■  A 

0  ■ 

M21 

M22 

0 

0 

(3-70) 


So,  using  (3-69), 


0 

5ii 

0 

5i2 

R 

2a^  F2P  F  OiP 

P  (2a  FP)  IT] 

0 

‘S'21 

S22  _ 

a  (4  —  2a  —  /3) 

[  P{2aFP)lT 

2P‘^IT^ 

-h- 


Q 


T^  (—2  -|-  a)  T  (—2a  —  P  +  aP  +  a^) 


(— 4a/3  -|-  aP^  +  2a^/3)  T  (—2a  —  P  +  aP  +  a^)  (—2/3  -|-  2aP  —  2g?  F  a^) 


0  0 
0  Q 


F 


A  0 
0  0 


In  particular. 


iSii  —  Mil  -|-  A 

R  (2c?  -|-  2/3  -|-  a/3)  QT^  (“2  -|-  a)  . 

I  “/  '  —  n  \  — A 


a 


and 


(4  —  2a  —  /3)  (— 4a/3  -|-  aP'^  F  2a‘^P) 

S21  =  M21 


(3-71) 
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_  R(3{2a  +  (3)  QT  iy—2a  —  (3  +  al5  +  a^)  79' 

“  a(4-2a-/3)r^  (-4a^  +  a/3^  +  2a2^)  ' 

We  now  turn  our  attention  to  (3-35).  We  first  simplify  the  summands  in  the  left  hand  side  of 
(3-35).  In  steady-state, 


HSH'  +  i?  =  [  1  0  ] 


"  0  o' 

Sll  5i2 

■  1  ■ 

0  0 

5'21  S'22 

0 

-|-  i?  -|-  A 


—  /Sii  -|-  ii  -|-  A  . 


Also, 


and 


ffl-OA  =  [  1  0] 


1 

Si 

; — 1 

1 _ 

1 - 

1—1 

1 

0  1 

0 

•A- 1-1  = -A 


kD%'H'  =  -A  . 

oX  } 


For  the  right  hand  side  of  (3-35)  in  steady-state 

SB'  = 


5ll  5i2 

O  O 

*S'l2  S'22 


and 


^Dk  (  )  = 


1  T 
0  1 


-1 

0 


511 

o 

512 


•  A  •  1  •  1  = 


-A 

0 


Substituting  (3-73),  through  (3-77)  into  (3-35)  gives 

(^5ii+i7  +  A-A-A^  = 

This,  written  as  two  scalar  equations, 

a  (  Sii  +  i?  +  A  -  A  -  A  )  =  a  (  Sii  +  i?  -  A  )  =  -  A 


'  0 

Sii 

■  -A  ■ 

0 

0 

.  -^12  _ 

(3-73) 

(3-74) 

(3-75) 

(3-76) 

(3-77) 


(3-78) 


/? 


/3 


^(5ii+i7  +  A-  A-  A)=^(5ii  +  R-  A)=5 


>12 


Substituting  (3-71)  and  (3-72) 

R  (20?  +  2(3  +  a/3) 


a 


gr2  (-2  + 


a 


a  (4  —  2a  —  ,5)  (— 4a,0  -|-  af3‘^  +  20^(3') 


-F  A  +R-  k 


R  (2a2  +  2/3  +  aP)  QT^  (-2  +  a) 

a  (4  —  2a  —  /3)  (— 4a/3  -|-  a/3^  -|-  2a^/3) 


(3-79) 
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J(R{2a^  +  2l3  +  a(3)  ^  QT^-2  +  a) 

yy  a  (4  —  2a  — /3)  (— 4a/3  +  a/3^  +  2a2/3)  J 

/  i?/3  (2a  +  P)  QT  (—2a  —  /3  +  otP  +  a^)  \  ^ 

^a  (4  —  2a  — /3)  T  (— 4a/3  +  a/3^  +  2a^/3)  J 

We  will  later  remark  on  the  fact  that  A  cancels  out  at  this  point: 


/  R  (2a^  +  2/3  +  a/3) 
y  a  (4  —  2a  —  /3) 


QT^  (-2  +  g) 

(— 4a^  +  aP'^  +  2a^P') 


R  (2a^  +  2/3  +  a/3)  QT^  (“2  +  a) 

a  (4  —  2a  —  /3)  (— 4a/3  +  a/3^  +  2a‘^P) 

/ R  (2a^  +  2^  +  aP)  QT^  (-2  +  a)  \ 

y  a(4  — 2a  — /3)  (— 4a/3  +  a/3^  +  2a^/3)  J 

_  RP  (2a  +  /3)  QT'^  (-2a  -  /3  +  a/3  +  a^) 
a  (4  —  2a  —  /3)  (— 4a/3  +  a/3^  +  2a‘^P) 

We  define  =  QT^ / R.  With  Q  in  (m/  sec)^,  T  in  seconds  and  R  in  p  is  unitless. 
Substituting  this  in  the  previous  two  equations,  we  obtain 


/  (2a^  +  2/3  +  a/3)  (-2  +  a) 

y  a  (4  —  2a  —  ,5)  (— 4a,0  +  a/3^  +  2a^P) 


_  (2a2  +  2^  +  a^)  p2  (_2  + 

“  a  (4  -  2a  -  /3)  ^  (-4a/3  +  a/3^  +  2a2/3) 

/  (2a^  +  2^  +  a/3) _ p^  (-2  +  a) _ \ 

y  a  (4  —  2a  —  /3)  (— 4a/3  +  a/3^  +  2a2/3)  y 

_  /3  (2a  +  P)  p^  (-2a  -  P  +  aP  +  a^) 

a  (4  —  2a  —  /3)  (— 4a/3  +  aP"^  +  2a2/3) 

We  divide  the  previous  two  equations  and  cancel  an  a, 

a  (2a^  +  2/3  +  a/3)  (-4/3  +  /3^  +  2a/3)  +  p^  (-2  +  a)  (4  -  2a  —  /3) 

P  P  (2a  +  P)  (—4,5  +  /3^  +  2a,5)  +  p2  (—2a  —  ,5  +  a/3  +  a^)  (4  —  2a  —  /3) 

Cross  multiplying  gives: 

2/3^  +  (4a  -  8)  13^  +  p^  (a^  -  2a  +  2)  p'^+ 

p2  (Sa^  -  lOa^  +  12a  -  8)  /3  +  p^  (2a^  -  80^  +  8a2)  =  0  .  (3-80) 

Equation  (3-80)  gives  our  relationship  between  a  and  /3.  The  noise  ratio  p,  a  parameter  in  the 
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equation,  is  known,  or  known  to  be  within  a  range.  Equation  (3-80)  may  be  factored® 

(/3  -h  (2q;  -  4))  (2/3^  -k  ((a^  -  2a +  2)  13  +  {a  -  2)))  =  0  .  (3-81) 

Hence,  /3  =  (4  —  20;)  is  a  solution  that  is  independent  of  p.  This  solution  is  not  permitted 
since  it  violates  Condition  3  of  (3-64).  There  is  a  second  real  solution  for  /3  given  a  (which 
depends  on  p.)  The  remaining  two  solutions  for  f3  given  a  may  be  a  complex  conjugate  pair. 
The  table  below  gives  representative  solutions  to  (3-80).  The  two  real  solutions  are  presented. 
We  do  not  use  the  second  one  listed  because  of  Condition  3.  Appendix  B  gives  the  solution  to 
the  cubic  equation  part  of  (3-81).  Of  course,  the  Newton-Raphson  method  may  be  used  to 
compute  all  of  the  solutions  to  (3-80). 


a 

/3 

2 

0.2 

0.04385,  3.6 

4 

0.2 

0.04386,  3.6 

6 

0.2 

0.04389,  3.6 

6 

0.4 

0.1866,  3.2 

8 

0.2 

0.04389,  3.6 

8 

0.4 

0.1870,  3.2 

10 

0.2 

0.04389,  3.6 

10 

0.4 

0.1873,  3.2 

10 

0.5 

0.2959,  3.0 

These  a  and  (3  give  that  the  eigenvalues  of  F,  in  (3-52),  have  norm  less  than  1.  Hence,  by 
Theorem  2.1,  page  64  of  Anderson  and  Moore  [1],  Mu  and  Mq,  the  solutions  to  (3-56)  and 
(3-57)  respectively,  exist,  are  unique,  and  are  positive  definite. 

Case  b:  Rf,  =  [  0  T  ]' 


We  consider  the  development  of  this  section  using  (3-40),  that  is  R  =  Rb.  The  effect  of 
(3-40)  on  (3-15)  is 


777,  =  ^Bm  = 


■  1 

-T  ■ 

■  0  ■ 

m  = 

rj^2 

0 

1 

T 

T 

Continuing,  we  observe  that,  roughly  speaking,  we  replace  every  “Q”  in  the  above 
development  with  a  “QT^”.  Specifically,  (3-48)  becomes 


Q  =  4>"^RQR'4>"^' 


J.2  _rp 

-T  1 


(3-82) 


Also,  (3-59)  becomes 


fqf' 


0  0 
0  1 


g-T^ . 


(3-83) 


In  the  development  involving  Mg,  (3-60)  through  (3-66),  we  multiply  every  g  by  a  T^.  Doing 
this,  (3-66)  becomes 


''Provided  by  Armido  R.  DiDonato. 
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_  _ Q-T^ _  r  T2  (-2  +  5a  -  4a^  +  a^) 

^  (— 4a/3  +  a/3^  +  2a^l3)  T  (—2a  +  /3  —  af3  +  3a^  —  a^) 

T  (—2a  +  P  —  a/3  +  3a^  —  a^) 

(-2^  +  2a/3  -  2a2  +  a^) 

_  O 

These  changes  continue  on  to  M  and  M,  giving  that  (3-69)  becomes 


(3-84) 


M  = 


R 


a  (4  —  2a  —  /3) 


2a^  +  2P  +  a/3 
P  (2a  +  P)  /T 


P{2a  +  P)/T 

2P‘^/T^ 


Q-T^ 

(— 4a^  -|-  aP'^  +  20^0) 


r2(-2  +  a) 


T  (—2a  —  P  +  OiP  +  a^) 


0  0 
0  1 


•T^Q  . 


T  (—2a  —  P  +  aP  +  a^) 
(-2/3 -h  2a/3  -  2a2 -h  a^) 


Finally,  looking  at  the  effect  of  (3-40)  on  the  steady-state  time-update  total  covariance, 
putting  in  T^Q  for  Q  in  (3-71)  we  obtain 


S'!!  =  Mil  +  A 


R  (2c0  -|-  2/3  -|-  a/3)  QT'^  (“2  -|-  a) 

a  (4  —  2a  —  /3)  (— 4a/3  -|-  aP"^  +  2a^P^ 

and  doing  the  same  in  (3-72)  gives 

521  =  Af21 

RP  (2a  -|-  P')  QT^  (—2a  —  P  O-P  -\-  a^) 
a  (4  —  2a  —  /3)  T  (— 4a/3  -|-  aP^  -|-  2a^P') 

Substitution  of  (3-85)  and  (3-86)  into  (3-78)  and  (3-79)  gives 


(3-85) 


(3-86) 


a 


/ R  (2a2  +  2/3  +  a/3)  QT^  (-2  +  a) 

y  a  (4  —  2a  —  /3)  (— 4a/3  -|-  aP^  +  2a^/3) 


R  (2a^  +  2;9  +  a/3)  QT^  (-2  +  a) 

a  (4  —  2a  —  ^)  (— 4a,0  -|-  a/3^  -|-  20^/3) 


/ R  (2a2  +  2P  +  aP)  QT^  (-2  +  a) 

a  (4  -  2a  -  /3)  ^  (-4a/3  +  a/3^  +  2a2/3) 


RP  (2a  -I-  P)  QT^  (-2a  -  P  +  aP  +  a^) 
a  (4  —  2a  —  /3)  (— 4a/3  -|-  a/3^  -|-  2a‘^P) 


This  time  we  define  =  QT^ / R.  With  Q  in  (m/  sec^)^,  T  in  seconds,  and  R  in  mP,  p  is 
again  unitless.  We  again  obtain  the  result  (3-81),  but  with  p^  interpreted  differently. 
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Case  c:  Bc=[  T"^ /2  T  ]' 


We  consider  the  development  of  this  section  using  (3-41),  that  \s  B  =  Be-  The  changes  are 
more  extensive  than  they  were  before  with  (3-40).  The  effect  of  (3-41)  on  (3-15)  is 


m  =  ^Bm  = 


■  1  -T  ' 

■  T‘^/2  ■ 

TJl  - 

■  -T‘^/2  ' 

0  1 

T 

III  - 

T 

ra 


For  (3-48), 

Q  =  ^-^BQB'^-^'  = 
Continuing,  we  find 

fqf'  = 


T 


Q  [  -r72  T  ]  = 


rV4  -r/2 

-T/2  1 


Q-T^ 


1  —  a  (1  —  a)  T 

rV4  -T/2  ■ 

g-r2 

1  —  a  (1  —  a)  T 

.  -P/T  1-/3 

-r/2  1 

.  -P/T  l-P 

T^{l-af  T{l-a){2-P) 
Til-a)i2-P)  (/3-2)2 

and  this  is  considerably  different  from  (3-59).  As  before,  (3-63), 


QT^ 


rniiQ 

■  l-(l-a)2 

-2(1  -  a)2r 

1 - 

(N 

bs 

(M 

1 

1 — 1 

1 

rni2Q 

= 

(l-a)/3/r 

l-{l-a){l-2P)  -{l-a){l-P)T 

_  FI22Q 

L  -(/32/r2) 

-2/3  (/3  -  1)  /T 

1  -  (1  -  2/3  +  /32) 

r2  (1  -  af 
T{l-a)  (2-/3) 
(/3  -  2)2 


gr2 


As  before,  the  matrix  that  is  inverted  is  referred  to  as  "A",  and  we  require  the  same 
conditions  on  its  determinant,  which  are  given  in  (3-64).  We  find  that 


Mq  = 


QT^ 


(8  -  20a  -2P  +  4a^  -F  lOa^  -  da^  -  2a^p) 


4  (4a/3  —  a/32  _  2q/2^)  T  (8a  —  4/3  -|-  4a/3  —  12a^  -|-  4a^  +  j3‘^  —  0.0^^ 

T  (8a  -4/3-1-  4a/3  —  12a^  -|-  4a^  +  [3"^  —  a/32) 

8/3  —  16a/3  -|-  8a^  —  4a^  —  2j3‘^  +  3a[3‘^  +  4a^/3 

In  this  case,  we  obtain  M  from  (3-55),  (3-58)  and  (3-88): 


M  = 


R 


2a2  +  2^-3a/3  /3  (2a  -  ^) /T 


a{A-2a-0[  /3  {2a  -  P) /T  2P0T^ 


(3-87) 


(3-88) 


QT^ 


8T2  -  20r2a  -  2T‘^P  +  AT'^aP  +  lOT^a^  -  dT^a^  -  2T‘^a‘^P 
4  (4a/3  -  a/32  _  2q,2^)  [  STa  -  ATp  -h  ATaP  -  12Ta‘^  +  4Ta^  +  Tp‘^  -  TaP‘^ 


8Ta  -  ATP  +  ATap  -  l2Ta^  +  ATa^  +  Tp^  -  TaP^ 
8/3  —  16a/3  -|-  8a^  —  4a^  —  2/32  -|-  3aP‘^  +  4a^/3 


(3-89) 
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Moving  on  to  the  time- updated  steady-state  covariance,  we  first  compute 

g^2  _  4^.2 Q,  _  2T‘^j3  _  4T‘^al3  -h  +  2T‘^a‘^l3 

8Ta  +  4r/3  -  12ra,0  -  4Ta^  -  +  2TaP^  +  ATa'^P 

8Ta  +  4T/3  -  12Ta/3  -  ATa'^  -  +  2TaP'^  +  ATa'^P  ' 

8/3  —  16q!/3  -|-  8a^  —  Aa^  —  20^  +  8aP‘^  +  4a^/3 

and  note  that  is  the  same  as  before.  Following  (3-68)  we  obtain 

2a2  +  2/3  +  a/3  /3  (2a  +  /3)  /T  ' 

P{2a  +  P)IT  2P‘^/T^ 


M  = 


R 


a  (4  —  2a  —  /3) 


4>Mq«F'  = 


QT^ 


A  (AaB  —  aB"^  —  2a^B) 


+  - 


Q-T^ 


8T2  -  4r2a  -  2T2/3  -  AT'^aP  -h  T'^aP'^  +  2T'^a‘^P 

.,2  rro2  ,  ^rp^o2  ,  Arp  2, 


A  (AaP  -  aP^  -  2a^p)  [  8Ta  +  ATp  -  UTaP  -  ATa^  -  Tp^  +  2TaP^  -F  ATa^P 


8Ta  +  ATP  -  12Tap  -  ATa^  -  Tp'^  +  2TaP^  +  ATa^P 
8/3  —  16a/3  -|-  8a^  —  4a^  —  2p‘^  -|-  3a,5^  -|-  4a^,5 


CO 

-h 

IrS  j^2 

We  repeat  (3-70) 


S  = 


Mil 

Mi2 

-h 

'AO' 

M21 

M22 

0  0 

and  then  substitute  from  (3-90) 


Q 


(3-90) 


0 

.Sii 

0 

*912 

R 

2a^  +  2P  +  aP 

P  {2a  +  P)/T] 

0 

_  ^21 

S22  _ 

a  {A  —  2a  —  P) 

[  P{2a  +  P)/T 

2P‘^/T'^ 

Q-T^  r  8T2  -  4r2a  -  2T^P  -  AT^aP  +  T'^aP^  +  2T‘^a^P 

^  A  {AaP  -  aP^  -  2a^p)  [  8Ta  +  ATp  -  UTap  -  ATa^  -  Tp^  +  2TaP‘^  +  ATa^P 

8Ta  +  ATP  -  12TaP  -  ATa"^  -  Tp^  +  2TaP^  +  ATa'^P  ' 

8/3  —  16a/3  -|-  8a^  —  4a^  —  2p'^  +  8aP‘^  +  Aa^P 


\ 

1 

'AO' 

+ 

4 

It** 

2 

rji2 

Q  A- 

0  0 

In  particular  (as  before) 

_  R  (2a2  +  2/3  +  a/3)  {8T‘^ 

a  (4  —  2a  —  /3) 

and 


5ii  =  Mil  +  A 

AT^a-2T‘^P  -  AT'^aP  +  T'^aP'^ +  2T‘^a^P)  1  ^ 

4  {AaP  -  aP^  -  2a‘^p)  +  4^  ^  ^ 

(3-91) 

*921  =  M21 


RP  {2a  +  P)  QT‘^  {8Ta  +  4r^  -  l2TaP  -  ATa'^  -  Tp"^  +  2TaP^  +  ATa'^p)  T^Q 
a  (4  —  2a  — /3)  T  4  (4a/3  —  a/3^  —  2a^/3)  2 

(3-92) 
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Substituting  (3-91)  and  (3-92)  into  (3-78)  and  (3-79),  we  obtain 

/  R  (2a^  +  2/3  +  aP)  QT^  (8  -  4a  -  2,g  -  4a/3  +  a/3^  +  2a'^0)  1 

y  O'  (4  —  2a  —  /3)  4  (4a/3  —  a/3^  —  2a'^P)  4 

_  /  R  (2a2  +  2P  +  aP)  QT^  {8  -  Aa  -  2P  -  4a/3  +  0/3^  +  2a2/3)  1  ^  \ 

y  a  (4  —  2a  —  ,5)  4  (4a/3  —  a/3^  —  2a2/3)  4 

and 

f  R{2a‘^  +  2p  +  aP)  QT^  {8  -  Aa  -  2P  -  4aP  +  aP^ +  2a‘^p)  1  ^ 

^  Y  a  {A  —  2a  —  P)  4  (4a/3  —  aP'^  —  2a^/3)  4  ^ 


RP{2a  +  P)  QT^[8a  +  AP-l2aP-Aa^  -  P'^ +  2aP'^ +  Aa'^p)  T^q\  ^ 
a{A  —  2a  —  P)T  4  (4a/3  —  a/3^  —  2a^/3)  2  J 

Again  we  define  =  QT^/R.  With  Q  in  (m/ sec^)^,  T  in  seconds,  and  R  in  mP,  p  is  again 
unitless.  In  this  case,  p  is  referred  to  as  either  the  target  maneuver  index  or  the  target 
tracking  index]  see  [2].  Again  we  note  that  A  drops  out.  This  modifies  the  above  as  follows: 

/  (2a^  +  2P  +  aP)  p^  (8  —  4a  —  2/3  —  4a/3  -|-  a/3^  -|-  2a^/3)  1  2 
y  a(4  — 2a  — /3)  4  (4a/3  —  a/3^  —  2a2/3)  4^ 


/  (2a^  +  2P  +  aP)  p^  (8  —  4a  —  2/3  —  4a/3  -|-  a/3^  -|-  2a^/3)  1  2^ 

y  a  (4  —  2a  — /3)  4  (4a/3  —  a/3^  —  2a2/3)  4^  J 

and 

/  (2a^  +  2P  +  aP)  p^  (8  —  4a  —  2,0  —  4a/3  -|-  a/3^  -|-  2a^/3)  1  2  \ 

y  a  (4  —  2a  —  /3)  4  (4a,0  —  aP‘^  —  2a^P)  4^  y 

(  P  (2a  -I-  P)  p^  (8a  +  AP  —  12a/3  -  4a^  —  P"^  +  2a/3^  -|-  4a^/3)  1 

"  (a(4-2a-^)  +  4  (4a/3  -  aP^  -  2a^p)  ^  2^  j  ' 

We  divide  the  previous  two  equations, 

/  (2a^  +  2P  +  aP)  p^  (8  —  4a  —  2/3  —  4a/3  -|-  a/3^  -|-  2a^/3)  1  2^ 

Q,  y  a  (4  —  2a  — /3)  4a/3  (4  — /3  —  2a)  A^  J 

f  P  (2a  -I-  /3)  p^  (8a  +  AP  -  12a/3  -  4a^  —  P‘^  +  2aP‘^  +  Aa^P)  1  2^ 
(^a(4-2a-/3)  ^  4a/3  (4  -  /3  -  2a)  ^  2^  J 

(4  -  2a  -  /3)  ^  AP{A-P-  2a)  ^  4^  “J 

/  /3  (2a  -I-  P)  p^  (8a  +  AP  —  12aP  —  Aa^  —  P'^  +  2a/3^  -|-  Aa^P)  1  „  \ 
\{A-2a-P)  ^  AP{A-P-  2a)  ^  2^  “j 
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/  4/3  (2a^  +  2/3  +  a^)  (8  —  4a  —  2/3  —  4a/3  +  a/3^  +  2a^/3)  „ 

1,  (4-2a-/3)  +  (4-/3-2a)  +  ^  J 

(  4/3^  (2a  +  /3)  ,  (8a  +  4/3  -  12a/3  -  4a^  -  +  2a^^  +  4a^/3)  ,  ^  ^ 

l^(4-2a-/3)+  (4-/3-2a)  +  ^ 

4/3  (2a^  +  2/3  +  a/3)  +  /?^  (8  —  4a  —  2/3  —  4a/3  +  a/3^  +  2a^/3)  +  p‘^al3  (4  —  2a  —  /3) 

“  4/3^  (2a  +  /3)  +  /o2  (8a  +  4^  -  12a/3  -  4a2  -  /S^  +  2a^^  +  4a2/3)  +  2/o2a/3  (4  -  2a  -  /3) 

4/3  (2a2  +  2/3  +  a/3)  +  2p2  (4  -  /3  -  2a) 

4^^  (2a  +  /3)  +  (8a  +  Af3  —  4a/3  —  4a^  —  /3^) 

Therefore, 

a  (4/3^  (2a  +  /3)  +  (8a  +  4/3  —  4a/3  —  4a^  —  /3^)) 

=  /3  (4^  (2a2  +  2/3  +  a,3)  +2p'^{A-P-  2a))  . 

Expanding, 

p^  {8aP  -8p  +  8a2  -  4a^  +  2/3^  -  a^^  -  Aa'^p)  -  8/3^  =  0  .  (3-93) 

Before  proceeding  with  a  numerical  solution  and  demonstration  of  this  equation,  we  will 
need  to  review  the  Kalata  relationship  [6] .  With  p  a  given,  there  are  two  equations  in  the  two 


unknowns  a  and  /3: 

/3  =  (4  —  2a)  —  4-v/l  —  a 

(3-94) 

and 

(4  -  2a)  -{4  +  p)  Vl-a  =  0  . 

(3-95) 

Using  (3-94),  we  can 

rewrite  (3-95)  as 

2  P" 

P  (l-a)' 

(3-96) 

Equation  (3-94)  gives  the  Kalata  relationship  between  a  and  (3.  When  we  know  the  correct  p, 
(3-96)  gives  the  applicable  a  and  /3.  Equations  (3-94)  and  (3-96)  are  in  [6];  see  page  176. 


/3  from  (3-93)  given  a  and  p 

P 

a 

/3 

2 

0.2 

0.044390 

2 

0.4 

0.19393 

4 

0.2 

0.044417 

4 

0.4 

0.19683 

6 

0.2 

0.044426 

6 

0.4 

0.19785 

8 

0.2 

0.044431 

8 

0.4 

0.19837 

10 

0.2 

0.044433 

10 

0.4 

0.19869 

10 

0.5 

0.32640 
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Optimal  a  and  /3  given  p  (3-93) 

a  —  fi  with  Kalata  relation 

a 

/3 

P 

a 

/3 

0.36 

0.08 

0.1 

0.36 

0.08 

0.50514 

0.17587 

0.25 

0.50514 

0.17587 

0.62837 

0.30481 

0.5 

0.62837 

0.30481 

0.75 

0.5 

1 

0.75 

0.5 

0.85410 

0.76393 

2 

0.85410 

0.76393 

0.92820 

1.0718 

4 

0.92820 

1.0718 

0.97871 

1.4590 

10 

0.97871 

1.4590 

As  p  approaches  infinity,  a  approaches  one  and  j3  approaches  two.  From  this  table,  we  see 
that  (3-93)  matches  the  Kalata  relationships  (3-95)  and  (3-96).  Mookerjee  and  Reifler  [7]  also 
got  the  Kalata  relationship  between  a  and  (3  for  their  problem  as  well.  Not  surprising,  since 
as  mentioned  above,  these  are  dual  problems. 


3-24 


NSWCDD/TR-12/555 


4  SUMMARY  AND  CONCLUSIONS 


In  this  report  we  considered  topics  related  to  determining  radar  sensor  bias.  We  presented 
an  algorithm  that  estimates  the  absolute  bias  of  two  sensors  when  the  relative  bias  between 
the  sensors  is  given.  The  algorithm  uses  the  relative  bias,  which  is  given  in  rectangular 
coordinates,  as  a  constraint.  The  absolute  biases,  in  spherical  coordinates,  for  the  sensors  are 
obtained  by  the  solution  to  an  optimization  problem  that  exploits  the  spherical-to-rectangular 
coordinate  conversion.  We  presented  a  reduced-state  filter  designed  for  performance  with 
sensor  bias.  The  filter  is  reduced-state  since  it  does  not  contain  additional  bias  states.  The 
filter  design  is  influenced  by  the  filter  in  Mookerjee  and  Reifler  [7]  and  may  be  viewed  as  a 
dual  design,  in  the  control  theory  sense,  to  their  filter  [7]. 

A  flow  diagram  for  processing  radar  data  with  bias  may  contain  these  stages: 

1.  Estimate  state  with  the  a  —  (5  filter  optimized  for  measurement  bias,  as  presented  in 
Chapter  3. 

2.  For  a  multi-sensor  problem,  estimate  the  relative  sensor  bias  using  an  optimized 
algorithm  such  as  in  Brown,  Weisman,  and  Brock  [5]. 

3.  Continue  by  estimating  the  absolute  bias  for  each  sensor  using  the  algorithm  presented 
in  Chapter  2. 
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APPENDIX  A: 

TRANSFORMATION  FROM  ENU(l)  TO  ENU(2) 
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In  this  appendix,  we  present  the  transformation  from  the  ENU(l),  East  North  Up,  to  the 
ENU(2)  coordinate  systems.  But  first,  consider  the  transformation  from  the  Earth  Centered 
Inertial,  denoted  ECI,  to  ENU.  Consider  an  ENU  coordinate  axis  located  at  longitude-latitude 
Q  —  L  and  define  the  rotation  matrix 


■  1 

0 

0 

cos  U 

0 

—  sin  U 

■  0 

1 

0  ■ 

TeCI2ENU  = 

0 

cos  L 

—  sinL 

0 

1 

0 

0 

0 

1 

0 

sinL 

cos  L 

sinU 

0 

cos  U 

1 

0 

0 

—  sin  Q  cos  U  0 

=  —  sin  L  cos  U  —  sin  L  sin  U  cos  L 

cos  L  cos  U  cos  L  sin  U  sin  L 

with  Tenu2ECI  =  '^ECI2ENU-  position,  we  need  to  include  a  translation,  so  that  for  a 
given  position  vector  in  ENU  coordinates 


Peci 


Tee 

sin^  L 


cos  L  cos  U 
cos  L  sin  U 
^1  —  e^)  sinL 


+  Tenu2ECiPenu 


where  r^e  is  the  earth’s  equatorial  radius  and  e  is  the  earth’s  eccentricity.  Eor  velocity,  use  the 
rotation  alone. 

Let  the  ENU(l),  ENU(2)  coordinate  system  be  located  at  longitude-latitude  {Ui,Li}  and 
{112,^2}  respectively.  Next  we  consider  our  transformation  going  from  {Ui,Li}  to  {U2,L2}- 
The  rotation  part  of  this  transformation  can  be  represented  by  the  matrix  below  (going  from 
ENU(l)  to  ENU(2)).  There  are  three  steps.  Eirst,  the  ENU(l)  coordinates  are  rotated  down 
to  the  equator.  Second,  these  coordinates  are  rotated  along  the  equator  by  the  longitude 
difference.  Third  is  the  rotation  up  to  the  latitude  of  the  ENU(2)  system. 


■  1 

0 

0 

cos  (U2 

-L!i) 

0 

—  sin  (U2  — 

TeNU{1)2ENU{2)  — 

0 

cos  L2 

—  sin  L2 

0 

1 

0 

0 

sin  L2 

cos  L2 

sin  (U2 

-Ui) 

0 

cos  (U2  — 

■  1  0 

0 

X 

0  cos  L 

sinLi 

0  —  sinLi 

cos  Li 

cos(U2  — Ui)  sin  Li  sin  (U2  —  Ui) 

—  sinL2  sin  (U2  —  Ui)  cosLi  cos  L2  +  sinLi  sinL2  cos  (U2  —  Ui) 
cos  L2  sin  (U2  —  Ui)  cos  Li  sin  L2  —  cos  L2  sin  Li  cos  (U2  —  Ui) 


—  cos  Li  sin  (U2  —  Ui) 
cos  L2  sin  Li  —  cos  Li  sin  L2  cos  (U2  —  ) 

sin  Li  sin  L2  +  cos  Li  cos  L2  cos  (U2  —  Ui) 

(Note 

cos(U2  — 0  —  sin(U2  — 

0  1  0 
sin(U2  — Ui)  0  cos(U2  — Ui) 
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cos  cos  ^2  +  sin  sin  ^2  0  —  cos  sin  ^2  +  cos  ^^2  sin 
0  10 
cos  Hi  sin  H2  —  cos  H2  sin  Hi  0  cos  Hi  cos  H2  +  sin  Hi  sin  H2 


cos  H2 

0 

—  sin  H2 

cos  Hi 

0 

sin  Hi 

0 

1 

0 

0 

1 

0 

sin  H2 

0 

cos  H2 

—  sin  Hi 

0 

cos  Hi 

so  that  the  rotation  T^nu{i)2ENU{2)  is  a  rotation  from  the  first  coordinates  down  to  ECI  and 
then  up  to  the  second  coordinates.)  We  have 


TeNU{2)2ENU{1)  —  '^ENU{l)2ENU{2) 


is 


The  position  vector  from  the  ENU(l)  to  the  ENU(2)  coordinate  axes  (in  ECI  coordinates) 


PeNU{1)2ENU{2),ECI 


cos  L2  cos  H2 

cos  Li  cos  Hi 

cos  L2  sin  H2 

cosLi  sin  Hi 

(1  —  e^)  sinL2 

(1  —  e^)  sinLi 

\/l  —  sin^  L2 

•  ee 

\J\  —  sin^  Li 

and,  in  the  other  coordinates,  this  vector  is 


PENU{l)2ENU{2),ENU{i)  —  TECI2ENU{i)PENU{l)2ENU{2),ECI 
The  total  position  coordinate  transformation,  including  translation,  can  be  represented  by 

PeNU{2)  =  —PeNU{1)2ENU{2),ENU{2)  +  TeNU{1)2ENU{2)PeNU{1) 

The  total  velocity  coordinate  transformation  is  given  by  the  rotation  alone. 
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APPENDIX  B: 

SOLUTION  TO  THE  CUBIC  EQUATION 
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In  this  appendix,  we  present  the  solution  to  the  cubic  polynomial  equation  and  apply  the 
solution  to  the  cubic  equation  that  arises  from  our  tracking  problem.  But  first,  we  consider 
the  stability  of  our  filter.  The  closed  loop  state  transition  matrix  is 


'IT' 

a 

1  —  a  T 

0  1 

(5  IT 

[10]  = 

-P/T  1  _ 

hence,  the  eigenvalues  of  ^CL  are 


and 


For  the  stability  of  the  filter,  we  need  the  absolute  value  of  ei,  62  less  than  one.  If  /3  <  0,  no 
matter  what  a  is,  at  least  one  of  ei,  62  will  have  an  absolute  value  of  one  or  more;  hence,  a 


requirement  for  the  stability  of  the  filter  is  /3  >  0.  This  is  the  same  story  if  a  <  0.  If  a  >  2 
then  (1  —  a;/2)<— 1  and  the  radical  causes  either  the  absolute  value  ei  or  62  to  be  one  or 


greater.  Also,  for  this  case,  depending  on  /3,  if  the  radical  is  complex,  both  ei  and  62  would 
have  absolute  value  greater  than  one.  In  summary,  necessary  conditions  for  stability  are: 

i.  0  <  a  <  2 

ii.  0  <  /3. 

Mentioned  in  Chapter  3  was  the  noise  reduction  factor,  which  is  the  ratio  of  the 
steady-state  position  covariance  and  the  measurement  noise  intensity;  see  Bar-Shalom,  Li,  and 
Kirubarajan  [B-1].  For  this  problem  the  noise  reduction  factor  works  out  as  a.  For  the  filter 
to  produce  noise  reduction,  which  means  the  noise  reduction  factor  must  be  less  than  1,  we 
add  the  condition: 

iii.  a  <  1. 

The  canonical  form  for  the  cubic  equation  is  [B-2] 


+  py  +  q  =  0. 


(B-1) 


While  equation  (B-1)  seems  to  be  less  general  than 

+  az^  +  bz  +  c  =  Q  ; 


(B-2) 


it  is  not  since  (B-2)  may  be  reduced  to  (B-1)  using  the  transformation 


a 


(B-3) 


Substituting  (B-3)  into  (B-2),  we  obtain 
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=  -  ay'^  +  ^a^y  -  +a(^‘^  -  ‘^ay  +  ^  ^  ~  ^  ^ 


Letting 


and 


we  reduce  (B-2)  to  (B-1). 
Returning  to  (B-1),  define 


P  =  i.- j 


2a^  ab 


V 

w - =  y 

Zw  " 


Substituting  (B-6)  into  (B-1)  we  have 


w 


--) 

3w/ 


1  Ip' 


=  I  w  -  pw  +  - - T^^  +Pl'W-—  +q 


P 


3  w  27 


=  w^  -  ^^+q  =  0  . 
27 


3w 


Multiplying  through  by  we  get 


w^  +  qw^  -^=0 


Equation  (B-7)  is  quadratic  in  w^,  which  has  the  solution  (in  terms  of  w^) 


(B-4) 

(B-5) 


(B-6) 


(B-7) 


w 


3 


4p3 

~w  ■ 


Taking  the  cube  root  of  (B-8)  we  get 


w  = 


(B-8) 


(B-9) 


The  cube  root  is  a  triple  values  function  and  we  use  the  root  which  results  in  the  a  satisfying 
the  three  conditions  mentioned  in  the  top  paragraphs.  Back-substituting  w  into  (B-6)  to 
obtain  y,  and  the  y  into  (B-3),  we  obtain  z,  the  solution  to  our  cubic  equation  (B-2). 

Our  cubic  polynomial  of  interest  is  (3.92).  Repeating, 

{8al3  -  8/3  +  8a2  _  4a^  +  2/3^  -  a/3^  -  Aa'^0)  -  8/3^  =  0  .  (B-10) 

Equation  (B-10)  is  a  cubic  polynomial  equation  in  a  and  /3.  We  present  the  solutions  for  a  in 
terms  of  /3  and  /3  in  terms  of  a.  First,  we  solve  (3  =  f3  (a),  which  is  usually  desired  in  practice. 
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We  rewrite  (B-10)  as 


a  1 


ot 


+  -  --  )/3"  +  p"(  l-a  +  -)/3  +  p^(  -  -a^)  =  0 


or 


(B-11) 


Equation  (B-11)  is  in  the  form  of  (B-2),  so  we  use  (B-4)  and  (B-5)  to  reduce  it  into  the  form 
of  (B-1).  We  have 


a 


p  =  p^(l-a  +  -)--(p^(--- 


1 


a  1 


and 


a  1 
I  8  ~  4 


1  A  f  a  1 

oP 


a 


1  —  (x  p  (  ——  —  (X 


a 


Then,  combining  (B-3)  and  (B-6),  we  get 


o  pa 

p  =  w - 

3n;  3 

with  w  given  by  (B-9),  p  given  by  (B-12),  and  by  comparing  (B-11)  with  (B-2) 


(B-12) 

(B-13) 

(B-14) 


a  =  p 


a  1 
8  “  4 


Equation  (B-10)  is  a  cubic  in  both  a  and  /3.  Having  solved  (3  in  terms  of  a,  we  solve  for  a 
in  terms  oi  (3,  a  =  a  {/3).  Dividing  (B-10)  by  —4/3^  we  obtain 

1.0  1 


We  have 


and 


a"  +  (/3  -  2)  a"  +  (  -/3^  -  2/3  I  a  +  2^  -  -/3^  +  2/3  =  0 


P={^/32-2/3)-^(/3-2)2 


(B-15) 


q=^{f3-2f-^{P-2)(^f3^-2p)  +  (2^-^P^  +  2/3 


^3  _  1 

3 


p-  2^ 


Referring  to  (B-14),  we  then  find  that 


a  =  w  — 


p  a 
3w  3 

with  w  given  by  (B-9),  p  given  by  (B-15),  and  with 

a  =  (/3  -  2)  . 
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